test_puma_mdh_num_geo_kin_dyn.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "ab3555da",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n",
    "import numpy\n",
    "from math import cos, sin\n",
    "import math # for sympy >1.1.1 <=1.4\n",
    "\n",
    "import sympybotics\n",
    "from sympybotics._compatibility_ import exec_\n",
    "\n",
    "pi = sympy.pi\n",
    "q = sympybotics.robotdef.q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "ca04d5e7",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma560_def = sympybotics.RobotDef(\n",
    "    'Puma Robot 560 mdh',\n",
    "    [(    0.,      0.,      0., q),\n",
    "     (-pi/2.,      0.,  0.2435, q),\n",
    "     (    0.,  0.4318, -0.0934, q),\n",
    "     ( pi/2., -0.0203,  0.4331, q),\n",
    "     (-pi/2.,      0.,      0., q),\n",
    "     ( pi/2.,      0.,      0., q)],\n",
    "    dh_convention='modified')\n",
    "\n",
    "puma560_def.frictionmodel = None\n",
    "\n",
    "puma560 = sympybotics.RobotDynCode(puma560_def)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "752ddcb7",
   "metadata": {},
   "outputs": [],
   "source": [
    "q_test = [0.7504516826728697, 0.8395156106908136, 0.16851233582594916, 0.3849629637427072, 0.5252993946810777, 0.6701207256444748]\n",
    "dq_test = [0.24721855939629367, 0.9805915670454258, 0.9895299755642817, 0.7861135739668947, 0.273842245476577, 0.17182358900767503]\n",
    "ddq_test = [0.707405815485141, 0.25295715193420953, 0.9763909835998361, 0.8412822676113918, 0.4867768296473465, 0.11480270540937143]\n",
    "\n",
    "dynparm_test = [0.0, 0.0, 0.0, 0.0, 0.0, 0.34999999999999998, 0, 0, 0, 0, 0.1350808, -0.0070992, 0.018931199999999999, 0.60891200000000001,\n",
    "                0.0016703999999999998, 0.62008400000000008, 1.1832, 0.10439999999999999, -0.2784, 17.4, 0.090460800000000008, 0.0, 0.0,\n",
    "                0.013440800000000001, 0.0047039999999999998, 0.089520000000000002, 0.0, -0.336, 0.0672, 4.8, 0.0020960200000000001, 0.0,\n",
    "                0.0, 0.0020960200000000001, 0.0, 0.0012999999999999999, 0.0, 0.0, -0.015579999999999998, 0.82, 0.00029999999999999997, 0.0,\n",
    "                0.0, 0.00029999999999999997, 0.0, 0.00040000000000000002, 0.0, 0.0, 0.0, 0.34, 0.00024216, 0.0, 0.0, 0.00024216, 0.0,\n",
    "                4.0000000000000003e-05, 0.0, 0, 0.0028799999999999997, 0.09]\n",
    "\n",
    "dynparm_test2 = [0.47804562306292275, 0.5871876506259908, 0.9487349009746813, 0.35387185413632094, 0.28071604959871554, 0.368556182617345,\n",
    "                 0.24355010647230801, 0.6753463418802456, 0.9728151452953864, 0.6620741264406734, 0.34669638996014096, 0.01593886435340608,\n",
    "                 0.3521748260592804, 0.5384045845183812, 0.021600503502885116, 0.4654003203805651, 0.5202014161122065, 0.33744920539722967,\n",
    "                 0.052363297799702835, 0.07051826001770234, 0.7389222546505236, 0.771434543548951, 0.3652539269897015, 0.2603059367721896,\n",
    "                 0.4310648491411889, 0.7071252186366607, 0.16320122542325732, 0.44948421506462655, 0.48085540250421277, 0.08408482356412372,\n",
    "                 0.923593157615906, 0.46852453511703684, 0.6670004526434297, 0.573634975268657, 0.12665814747855264, 0.3152822779549781,\n",
    "                 0.21725524421221942, 0.5727451645276381, 0.7984025459787872, 0.7630923700903489, 0.27670044727992893, 0.919667661316697,\n",
    "                 0.8828363383223908, 0.3618378476984413, 0.20690997158181246, 0.8321536435628591, 0.556153477173109, 0.3985225289975399,\n",
    "                 0.7128303168401632, 0.433898343199971, 0.35116090526732047, 0.551636779637059, 0.03397585970570116, 0.28333059714010744,\n",
    "                 0.4785548774382852, 0.16258401709779136, 0.8246056496848533, 0.23027686557606952, 0.26655111443159285, 0.6087446254045791]\n",
    "\n",
    "q_num_subs = dict(zip(puma560_def.q, q_test))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "f27e8bae",
   "metadata": {},
   "outputs": [],
   "source": [
    "M_func_def = sympybotics.robotcodegen.robot_code_to_func('python', puma560.M_code, 'M', 'M_puma560', puma560_def)\n",
    "c_func_def = sympybotics.robotcodegen.robot_code_to_func('python', puma560.c_code, 'c', 'c_puma560', puma560_def)\n",
    "C_func_def = sympybotics.robotcodegen.robot_code_to_func('python', puma560.C_code, 'C', 'C_puma560', puma560_def)\n",
    "g_func_def = sympybotics.robotcodegen.robot_code_to_func('python', puma560.g_code, 'g', 'g_puma560', puma560_def)\n",
    "tau_func_def = sympybotics.robotcodegen.robot_code_to_func('python', puma560.invdyn_code, 'tau', 'tau_puma560', puma560_def)\n",
    "H_func_def = sympybotics.robotcodegen.robot_code_to_func('python', puma560.H_code, 'H', 'H_puma560', puma560_def)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "b2c7f7f1",
   "metadata": {},
   "outputs": [],
   "source": [
    "l = locals()\n",
    "exec_(M_func_def, globals(), l)\n",
    "exec_(c_func_def, globals(), l)\n",
    "exec_(C_func_def, globals(), l)\n",
    "exec_(g_func_def, globals(), l)\n",
    "exec_(tau_func_def, globals(), l)\n",
    "exec_(H_func_def, globals(), l)\n",
    "tau_puma560 = l['tau_puma560']\n",
    "g_puma560 = l['g_puma560']\n",
    "c_puma560 = l['c_puma560']\n",
    "C_puma560 = l['C_puma560']\n",
    "M_puma560 = l['M_puma560']\n",
    "H_puma560 = l['H_puma560']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "e1459925",
   "metadata": {},
   "outputs": [],
   "source": [
    "assert_precision = 10"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "ed7bef39",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = puma560.geo.T[-1]\n",
    "T = T.subs(q_num_subs)\n",
    "T = numpy.matrix(T).astype(numpy.float64)\n",
    "\n",
    "T_pcorke = numpy.matrix([[-0.655113870655343, -0.474277925274361,  0.588120962109346,   0.368531204501578],\n",
    "                         [ 0.524340309498464,  0.275038163391149,  0.805866768463298,   0.548861637700235],\n",
    "                         [-0.543960528264717,  0.836310025215453, 0.0685017183295358, -0.0731561881633544],\n",
    "                         [               0.0,                0.0,                0.0,                 1.0]])\n",
    "\n",
    "assert not numpy.any(numpy.round(T_pcorke - T, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "1d4d6f35",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = puma560.kin.J[-1]\n",
    "J = J.subs(q_num_subs)\n",
    "J = numpy.matrix(J).astype(numpy.float64)\n",
    "\n",
    "J_pcorke = numpy.matrix([[   -0.548861637700235,   -0.053505039458511,    0.181558777632408,               0.0,                0.0,                0.0],\n",
    "                         [    0.368531204501578,  -0.0498902657753524,    0.169292757497222,               0.0,                0.0,                0.0],\n",
    "                         [ 2.66713734431434e-17,   -0.643843409557299,    -0.35547724614047,               0.0,                0.0,                0.0],\n",
    "                         [-2.77555756156289e-17,   -0.681969181663072,   -0.681969181663072, 0.618588326585383, -0.778592276991744,  0.588120962109346],\n",
    "                         [-1.04083408558608e-16,    0.731380909828662,    0.731380909828662, 0.576796808883883,  0.541217849732837,  0.805866768463298],\n",
    "                         [                  1.0, 5.72458747072346e-17, 5.72458747072346e-17, 0.533529683779323,  0.317611878460765, 0.0685017183295358]])\n",
    "\n",
    "assert not numpy.any(numpy.round(J_pcorke - J, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "896719dd",
   "metadata": {},
   "outputs": [],
   "source": [
    "tau = tau_puma560(dynparm_test, q_test, dq_test, ddq_test)\n",
    "tau = numpy.matrix(tau).T.astype(numpy.float64)\n",
    "\n",
    "tau_pcorke = numpy.matrix([[   2.71610302653938],\n",
    "                           [  -28.7944111980059],\n",
    "                           [  -7.07013200484716],\n",
    "                           [0.00682888994276653],\n",
    "                           [-0.0220889100229662],\n",
    "                           [5.59805923645946e-5]])\n",
    "\n",
    "assert not numpy.any(numpy.round(tau_pcorke - tau, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "92c36622",
   "metadata": {},
   "outputs": [],
   "source": [
    "g = g_puma560(dynparm_test, q_test)\n",
    "g = numpy.matrix(g).T.astype(numpy.float64)\n",
    "\n",
    "g_pcorke = numpy.matrix([[2.88527195030991e-15],\n",
    "                         [   -31.1492065095002],\n",
    "                         [   -7.04528104551999],\n",
    "                         [ 0.00449992182016002],\n",
    "                         [  -0.026719893613902],\n",
    "                         [                 0.0]])\n",
    "\n",
    "assert not numpy.any(numpy.round(g_pcorke - g, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "6d3cfeaf",
   "metadata": {},
   "outputs": [],
   "source": [
    "c = c_puma560(dynparm_test, q_test, dq_test)\n",
    "c = numpy.matrix(c).T.astype(numpy.float64)\n",
    "\n",
    "c_pcorke = numpy.matrix([[   0.634177626441184],\n",
    "                         [    1.12398760139525],\n",
    "                         [  -0.373925639216401],\n",
    "                         [-8.54577359118394e-5],\n",
    "                         [ 0.00162606594660281],\n",
    "                         [  1.1075729558421e-5]])\n",
    "\n",
    "assert not numpy.any(numpy.round(c_pcorke - c, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "ea0ddd0e",
   "metadata": {},
   "outputs": [],
   "source": [
    "C = C_puma560(dynparm_test, q_test, dq_test)\n",
    "C = numpy.matrix(C).reshape(puma560.dof, puma560.dof).astype(numpy.float64)\n",
    "C_dq = C * numpy.matrix(dq_test).T\n",
    "\n",
    "assert not numpy.any(numpy.round(C_dq - c, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "691f6138",
   "metadata": {},
   "outputs": [],
   "source": [
    "M = M_puma560(dynparm_test, q_test)\n",
    "M = numpy.matrix(M).reshape(puma560.dof, puma560.dof).astype(numpy.float64)\n",
    "\n",
    "M_pcorke = numpy.matrix([[   2.88092539107775,     0.454967124647697,   -0.0748835030743064,   0.00187610810095465,  0.00080480941146298,  2.74006873318143e-6],\n",
    "                         [  0.454967124647696,      2.16866309038072,     0.368189596401607, -0.000307104503389342,  0.00234804941178354,  7.53260796283046e-6],\n",
    "                         [-0.0748835030743065,     0.368189596401607,     0.315830104422493, -0.000267827816326753,  0.00156601844062265,  7.53260796283046e-6],\n",
    "                         [0.00187610810095465, -0.000307104503389342, -0.000267827816326753,   0.00169083802882858, 1.98357371583556e-20,   3.4606953693396e-5],\n",
    "                         [0.00080480941146298,   0.00234804941178354,   0.00156601844062265,  2.86112061631228e-20,           0.00064216, 2.44929359829471e-21],\n",
    "                         [2.74006873318143e-6,   7.53260796283046e-6,   7.53260796283046e-6,    3.4606953693396e-5, 2.44929359829471e-21,               4.0e-5]])\n",
    "\n",
    "assert not numpy.any(numpy.round(M_pcorke - M, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "1568fa56",
   "metadata": {},
   "outputs": [],
   "source": [
    "H = H_puma560(q_test, dq_test, ddq_test)\n",
    "H = numpy.matrix(H).reshape(puma560.dof, puma560.dyn.n_dynparms).astype(numpy.float64)\n",
    "\n",
    "assert not numpy.any(numpy.round(tau_pcorke - H * numpy.matrix(dynparm_test).T, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "a77e592a",
   "metadata": {},
   "outputs": [],
   "source": [
    "tau_t2 = tau_puma560(dynparm_test2, q_test, dq_test, ddq_test)\n",
    "tau_t2 = numpy.matrix(tau_t2).T.astype(numpy.float64)\n",
    "\n",
    "tau_assrt2 = numpy.matrix([[  5.46017281703168],\n",
    "                           [ -8.53813836226581],\n",
    "                           [-0.490552199963579],\n",
    "                           [  16.4258371315422],\n",
    "                           [  6.18803453956645],\n",
    "                           [  7.99017179545593]])\n",
    "\n",
    "assert not numpy.any(numpy.round(tau_assrt2 - tau_t2, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "727ee208",
   "metadata": {},
   "outputs": [],
   "source": [
    "g_t2 = g_puma560(dynparm_test2, q_test)\n",
    "g_t2 = numpy.matrix(g_t2).T.astype(numpy.float64)\n",
    "\n",
    "g_assrt2 = numpy.matrix([[-1.77635683940025e-15],\n",
    "                         [    -12.2719368424473],\n",
    "                         [    -5.98183554268347],\n",
    "                         [     18.5386690234033],\n",
    "                         [    0.512467781908107],\n",
    "                         [      7.9940463468124]])\n",
    "\n",
    "assert not numpy.any(numpy.round(g_assrt2 - g_t2, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "dac7e6fb",
   "metadata": {},
   "outputs": [],
   "source": [
    "c_t2 = c_puma560(dynparm_test2, q_test, dq_test)\n",
    "c_t2 = numpy.matrix(c_t2).T.astype(numpy.float64)\n",
    "\n",
    "c_assrt2 = numpy.matrix([[  3.69185531689127],\n",
    "                         [ -1.34087787458136],\n",
    "                         [  1.44951875057327],\n",
    "                         [ -2.77730540324791],\n",
    "                         [  3.31210163468715],\n",
    "                         [-0.788977149763209]])\n",
    "\n",
    "assert not numpy.any(numpy.round(c_assrt2 - c_t2, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "fe9e986b",
   "metadata": {},
   "outputs": [],
   "source": [
    "C_t2 = C_puma560(dynparm_test2, q_test, dq_test)\n",
    "C_t2 = numpy.matrix(C_t2).reshape(puma560.dof, puma560.dof).astype(numpy.float64)\n",
    "C_dq_t2 = C_t2 * numpy.matrix(dq_test).T\n",
    "\n",
    "assert not numpy.any(numpy.round(C_dq_t2 - c_t2, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "e227cc48",
   "metadata": {},
   "outputs": [],
   "source": [
    "M_t2 = M_puma560(dynparm_test2, q_test)\n",
    "M_t2 = numpy.matrix(M_t2).reshape(puma560.dof, puma560.dof).astype(numpy.float64)\n",
    "\n",
    "M_assrt2 = numpy.matrix([[  3.65840168922149, -0.555774017452474,    -1.39220275090878,  0.985927680308992, -0.439435427090482,    0.563816026114764],\n",
    "                         [-0.555774017452474,   5.33926241554461,     4.32747701210166,  -1.24139932228482,   1.97231403813762,   -0.207221260322988],\n",
    "                         [ -1.39220275090878,   4.32747701210166,     4.13345124166948,  -1.07958817736071,   1.65363538346271, -0.00546039833881495],\n",
    "                         [ 0.985927680308992,  -1.24139932228482,    -1.07958817736071,   1.75689881264024, -0.358749490679038,    0.276358110012213],\n",
    "                         [-0.439435427090482,   1.97231403813762,     1.65363538346271, -0.358749490679038,   1.67868951032001,    0.396167549434339],\n",
    "                         [ 0.563816026114764, -0.207221260322988, -0.00546039833881495,  0.276358110012213,  0.396167549434339,    0.162584017097791]])\n",
    "\n",
    "assert not numpy.any(numpy.round(M_assrt2 - M_t2, assert_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "8b8f49e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "assert not numpy.any(numpy.round(tau_assrt2 - H * numpy.matrix(dynparm_test2).T, assert_precision))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py38",
   "language": "python",
   "name": "py38"
  },
  "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.8.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}