doc/notebook/Kinematics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Using Kinematics from Reachy's URDF file"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "from reachy import Reachy\n",
    "\n",
    "r = Reachy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Getting the Kinematic Chain from Reachy\n",
    "\n",
    "We use the [Ikpy](https://github.com/Phylliade/ikpy) library to create the kinematic chain from the URDF. It uses:\n",
    "\n",
    "* the path to the URDF file\n",
    "* the base element of your chain\n",
    "* the translation vector for the end tip"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can check the kinematic chain:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for link in r.ik_chain._chain.links:\n",
    "    print(link)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Forward kinematic"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can then call the forward kinematic method by specifying all joint angle (in radians). You have to specify 9 joints in the case of Reachy: the 7 defined joints plus one virtual before and one virtual at the end.\n",
    "\n",
    "For instance, when setting all joints to 0 except the elbow pitch at $\\pi /2$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "np.set_printoptions(precision=3)\n",
    "\n",
    "target = r.ik_chain.forward_kinematics([0, 0, 0, -90, 0, 0, 0])\n",
    "print(target)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can then extract the rotation matrix:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rot = target[:3,:3]\n",
    "print(rot)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And the translation vector:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "trans = target[:3, -1]\n",
    "print(trans)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Inverse kinematics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also use the inverse kinematics to find the joint angles from a target. The target must be passed as a 4x4 homogeneous matrix."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rot = np.eye(3)\n",
    "trans = [0.28, -0.09, -0.30]\n",
    "\n",
    "target = np.eye(4)\n",
    "target[:3, :3] = rot\n",
    "target[:3, 3] = trans\n",
    "print(target)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "joints = r.ik_chain.inverse_kinematics(target, accurate=True)\n",
    "print(joints)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def goto(joints):\n",
    "    for i in range(len(joints)):\n",
    "        r.motors[i].goal_position = joints[i]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "goto(joints)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also specify an initial position for the inverse kinematics optimisation. This will drastically speed up the process and help find better solutions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "joints = r.ik_chain.inverse_kinematics(target, initial_position=[0, 0, 0, -90, 0, 0, 0], accurate=True)\n",
    "print(joints)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "goto(joints)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "r.ik_chain.end_effector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Interactive testing"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Safety first!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in r.motors:\n",
    "    m.moving_speed = 50\n",
    "    m.torque_limit = 75"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in r.motors:\n",
    "    m.compliant = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def goto(x, y, z):\n",
    "    rot = np.eye(3)\n",
    "    trans = [x, y, z]\n",
    "\n",
    "    target = np.eye(4)\n",
    "    target[:3, :3] = rot\n",
    "    target[:3, 3] = trans\n",
    "    \n",
    "    cur_pos = [m.present_position for m in r.motors]\n",
    "    \n",
    "    joints = r.ik_chain.inverse_kinematics(target, \n",
    "                                           initial_position=cur_pos)\n",
    "\n",
    "    for i in range(len(r.motors)):\n",
    "        r.motors[i].goal_position = joints[i]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "r.ik_chain.end_effector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "goto(...)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Error measurement"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(r.ik_chain.forward_kinematics([m.present_position for m in r.motors])[:3, 3])\n",
    "print(r.ik_chain.forward_kinematics([m.goal_position for m in r.motors])[:3, 3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}