examples/kinematics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Robot Kinematics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Initialize Robot Model\n",
    "- A robot model, at a very minimum, is a kinematic chain\n",
    "- The kinematic chain is defined by a series of parameters\n",
    "    - See [Modified DH parameters](https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters#Modified_DH_parameters) for more info"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pybotics.robot import Robot\n",
    "from pybotics.predefined_models import ur10\n",
    "\n",
    "robot = Robot.from_parameters(ur10())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Forward Kinematics\n",
    "- The forward kinematics (FK) refer to the use of the kinematic equations of a robot to compute the pose of the end-effector (i.e., 4x4 transform matrix) from specified values for the joint parameters (i.e., joint angles)\n",
    "- ELI5: Where am I with the given joint angles?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[    0.94003631,    -0.34106157,     0.00295846, -1124.43089247],\n",
       "       [   -0.00491322,    -0.02221388,    -0.99974117,  -355.10102738],\n",
       "       [    0.34103901,     0.93977846,    -0.02255757,  -148.49504763],\n",
       "       [    0.        ,     0.        ,     0.        ,     1.        ]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "np.set_printoptions(suppress=True)\n",
    "\n",
    "joints = np.deg2rad([5,5,5,5,5,5])\n",
    "pose = robot.fk(joints)\n",
    "display(pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Inverse Kinematics\n",
    "- The inverse kinematics (IK) makes use of the kinematics equations to determine the joint parameters that provide a desired position for the robot's end-effector\n",
    "- The default internal IK implementation uses scipy.optimize.least_squares with joint limit bounds\n",
    "- ELI5: What joint angles do I need to have this position?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([5.00000032, 4.97126286, 5.05937406, 4.95609777, 5.00005964,\n",
       "       5.01321487])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "solved_joints = robot.ik(pose)\n",
    "display(np.rad2deg(solved_joints))"
   ]
  }
 ],
 "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.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}