Skip to content

Instantly share code, notes, and snippets.

@mlaves

mlaves/incremental_ik-panda.ipynb

Last active November 25, 2024 13:20
Show Gist options
  • Save mlaves/a60cbc5541bd6c9974358fbaad9e4c51 to your computer and use it in GitHub Desktop.
Save mlaves/a60cbc5541bd6c9974358fbaad9e4c51 to your computer and use it in GitHub Desktop.
Download ZIP
Incremental Inverse Kinematics for Franka Emika Panda 7DoF Robot
Display the source blob
Display the rendered blob
Raw
incremental_ik-panda.ipynb
{
"nbformat": 4,
"nbformat_minor": 5,
"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.8.8"
},
"colab": {
"name": "incremental_ik-panda.ipynb",
"provenance": [],
"include_colab_link": true
}
},
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "view-in-github",
"colab_type": "text"
},
"source": [
"<a href=\"https://colab.research.google.com/gist/mlaves/a60cbc5541bd6c9974358fbaad9e4c51/incremental_ik-panda.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "4f74e0ab"
},
"source": [
"# Incremental Inverse Kinematics for Franka Emika Panda 7DoF Robot\n",
"\n",
"The 3D pose $ \\mathbf{x} \\in \\mathbb{R}^{6} $ of the end effector (EE) is given by\n",
"\n",
"$$\n",
"\\mathbf{x} = \\mathbf{f}(\\mathbf{q}) ~ ,\n",
"$$\n",
"\n",
"with $ n $ joint angles $ \\mathbf{q} $ and direct kinematics $\\mathbf{f} : \\mathbb{R}^{n} \\to \\mathbb{R}^{6} $.\n",
"We are interested in solving the inverse kinematics $\\mathbf{q} = \\mathbf{f}^{-1}(\\mathbf{x})$, which we can do analytically (if possible), numerically (e.g., solve non-linear least-squares with Levenberg-Marquardt algorithm) or, as described here, using *incremental inverse kinematics* (IIK).\n",
"\n",
"In IIK, we linearize the direct kinematics at the current joint angle configuration $\\mathbf{q}^{\\ast}$\n",
"\n",
"$$\n",
"\\Delta \\left. \\mathbf{x}\\right|_{\\mathbf{x}^{\\ast}} \\propto \\Delta \\left. \\mathbf{q}\\right|_{\\mathbf{q}^{\\ast}} ~ ,\n",
"$$\n",
"\n",
"which we can easily solve around $ (\\mathbf{x}^{\\ast}, \\mathbf{q}^{\\ast}) $ using the Jacobian $ \\mathbf{J}_{\\mathbf{f}} := ( \\partial f_{i} / \\partial q_{j} )_{i,j} $\n",
"\n",
"$$\n",
"\\Delta \\left. \\mathbf{x}\\right|_{\\mathbf{x}^{\\ast}} = \\mathbf{J}_{\\mathbf{f}}(\\mathbf{q}^{\\ast}) \\Delta \\left. \\mathbf{q}\\right|_{\\mathbf{q}^{\\ast}} ~ .\n",
"$$"
],
"id": "4f74e0ab"
},
{
"cell_type": "markdown",
"metadata": {
"id": "86a6d43c"
},
"source": [
"## Steps of Incremental Inverse Kinematics\n",
"\n",
"Given: target pose $ \\mathbf{x}^{(t)} $ \n",
"Sought: joint angles $ \\mathbf{q}^{(t)} $\n",
"\n",
"0. Define starting pose $ (\\mathbf{x}^{(0)}, \\mathbf{q}^{(0)}) $ and set up IIK: $ \\Delta \\left. \\mathbf{x}\\right|_{\\mathbf{x}^{(0)}} = \\mathbf{J}(\\mathbf{q}^{(0)}) \\Delta \\left. \\mathbf{q}\\right|_{\\mathbf{q}^{(0)}} $.\n",
"1. Determine the deviation $ \\Delta \\mathbf{x}^{(k)} $ relative to the target pose; e.g. $ (\\mathbf{x}^{(t)} - \\mathbf{x}^{(0)}) $.\n",
"2. Check for termination; e.g., $ \\max ( \\vert \\Delta x^{(k)}_{i,j} \\vert ) \\leq \\epsilon $.\n",
"3. Solve $ \\Delta \\mathbf{x}^{(k)} = \\mathbf{J}(\\mathbf{q}^{(k)}) \\Delta \\mathbf{q}^{(k)} $ for $ \\Delta \\mathbf{q}^{(k)} $, e.g., by $ \\Delta \\mathbf{x}^{(k)} = (\\mathbf{J}(\\mathbf{q}^{(k)}) \\mathbf{J}(\\mathbf{q}^{(k)})^{T})^{-1} \\Delta \\mathbf{q}^{(k)} $.\n",
"4. Calculate new joint angles $ \\mathbf{q}^{(k+1)} = \\mathbf{q}^{(k)} + \\Delta \\mathbf{q}^{(k)} $.\n",
"5. $ k \\leftarrow k + 1 $, go to 2."
],
"id": "86a6d43c"
},
{
"cell_type": "markdown",
"metadata": {
"id": "f4ebb171"
},
"source": [
"## Set Up Direct Kinematics and Solve Jacobian Symbolically\n",
"\n",
"In case of the Panda robot, we have $ n=7 $ active joint angles $ \\mathbf{q} = \\left[ \\theta_{1}, \\ldots, \\theta_{7} \\right]^{\\mathsf{T}} $.\n",
"We use `sympy` to solve the Jacobian symbollically."
],
"id": "f4ebb171"
},
{
"cell_type": "code",
"metadata": {
"id": "0bd23a9d"
},
"source": [
"from sympy import symbols, init_printing, Matrix, eye, sin, cos, pi\n",
"init_printing(use_unicode=True)"
],
"id": "0bd23a9d",
"execution_count": 1,
"outputs": []
},
{
"cell_type": "code",
"metadata": {
"id": "6c652242"
},
"source": [
"# create joint angles as symbols\n",
"\n",
"q1, q2, q3, q4, q5, q6, q7 = symbols('theta_1 theta_2 theta_3 theta_4 theta_5 theta_6 theta_7')\n",
"joint_angles = [q1, q2, q3, q4, q5, q6, q7]"
],
"id": "6c652242",
"execution_count": 2,
"outputs": []
},
{
"cell_type": "code",
"metadata": {
"id": "9b6430cb"
},
"source": [
"# construct symbolic direct kinematics from Craig's DH parameters\n",
"# see https://frankaemika.github.io/docs/control_parameters.html\n",
"\n",
"dh_craig = [\n",
" {'a': 0, 'd': 0.333, 'alpha': 0, },\n",
" {'a': 0, 'd': 0, 'alpha': -pi/2},\n",
" {'a': 0, 'd': 0.316, 'alpha': pi/2},\n",
" {'a': 0.0825, 'd': 0, 'alpha': pi/2},\n",
" {'a': -0.0825, 'd': 0.384, 'alpha': -pi/2},\n",
" {'a': 0, 'd': 0, 'alpha': pi/2},\n",
" {'a': 0.088, 'd': 0.107, 'alpha': pi/2},\n",
"]\n",
"\n",
"DK = eye(4)\n",
"\n",
"for i, (p, q) in enumerate(zip(reversed(dh_craig), reversed(joint_angles))):\n",
" d = p['d']\n",
" a = p['a']\n",
" alpha = p['alpha']\n",
"\n",
" ca = cos(alpha)\n",
" sa = sin(alpha)\n",
" cq = cos(q)\n",
" sq = sin(q)\n",
"\n",
" transform = Matrix(\n",
" [\n",
" [cq, -sq, 0, a],\n",
" [ca * sq, ca * cq, -sa, -d * sa],\n",
" [sa * sq, cq * sa, ca, d * ca],\n",
" [0, 0, 0, 1],\n",
" ]\n",
" )\n",
"\n",
" DK = transform @ DK"
],
"id": "9b6430cb",
"execution_count": 3,
"outputs": []
},
{
"cell_type": "code",
"metadata": {
"colab": {
"base_uri": "https://localhost:8080/",
"height": 98
},
"id": "80eb83ca",
"outputId": "1755a270-194c-44a0-a6b5-7269dbc99908"
},
"source": [
"# test direct kinematics\n",
"\n",
"DK.evalf(subs={\n",
" 'theta_1': 0,\n",
" 'theta_2': 0,\n",
" 'theta_3': 0,\n",
" 'theta_4': 0,\n",
" 'theta_5': 0,\n",
" 'theta_6': 0,\n",
" 'theta_7': 0,\n",
"})"
],
"id": "80eb83ca",
"execution_count": 4,
"outputs": [
{
"output_type": "execute_result",
"data": {
"text/latex": "$\\displaystyle \\left[\\begin{matrix}1.0 & 0 & 0 & 0.088\\\\0 & -1.0 & 0 & 0\\\\0 & 0 & -1.0 & 0.926\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right]$",
"text/plain": [
"⎡1.0 0 0 0.088⎤\n",
"⎢ ⎥\n",
"⎢ 0 -1.0 0 0 ⎥\n",
"⎢ ⎥\n",
"⎢ 0 0 -1.0 0.926⎥\n",
"⎢ ⎥\n",
"⎣ 0 0 0 1.0 ⎦"
]
},
"metadata": {
"tags": []
},
"execution_count": 4
}
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "2082f0ad"
},
"source": [
"## Reshape Direct Kinematics\n",
"\n",
"Let $ \\mathrm{DK} : \\mathbb{R}^{n} \\to \\mathbb{R}^{4 \\times 4} $ be the direct kinematics transformation matrix (EE pose matrix), with\n",
"\n",
"$$\n",
"\\mathrm{DK} =\n",
"\\left[\n",
"\\begin{matrix}\n",
"a_{11} & a_{12} & a_{13} & a_{14} \\\\\n",
"a_{21} & a_{22} & a_{23} & a_{24} \\\\\n",
"a_{31} & a_{32} & a_{33} & a_{34} \\\\\n",
"0 & 0 & 0 & 1\n",
"\\end{matrix}\n",
"\\right] ~ .\n",
"$$\n",
"\n",
"Instead of computing Euler angles from $ \\mathrm{DK} $, we simply crop the last row and flatten column-wise:\n",
"$$\n",
"\\mathrm{A} = \\left[ a_{11}, a_{21}, a_{31}, \\ldots, a_{34} \\right]^{\\mathsf{T}}, A \\in \\mathbb{R}^{12 \\times 1} ~ .\n",
"$$"
],
"id": "2082f0ad"
},
{
"cell_type": "code",
"metadata": {
"id": "c7bab991"
},
"source": [
"A = DK[0:3, 0:4] # crop last row\n",
"A = A.transpose().reshape(12,1) # reshape to column vector A = [a11, a21, a31, ..., a34]\n",
"\n",
"Q = Matrix(joint_angles)\n",
"J = A.jacobian(Q) # compute Jacobian symbolically"
],
"id": "c7bab991",
"execution_count": 5,
"outputs": []
},
{
"cell_type": "markdown",
"metadata": {
"id": "eaf8b589"
},
"source": [
"## From Symbolical to Numerical\n",
"\n",
"We can now move to numerical computation for speed reasons.\n",
"With `lambdify`, we can create `numpy` functions from `sympy` expressions.\n",
"`numba` JIT compiles those functions to native code (optional)."
],
"id": "eaf8b589"
},
{
"cell_type": "code",
"metadata": {
"id": "c7378849"
},
"source": [
"import numpy as np\n",
"from sympy import lambdify\n",
"from numba import jit\n",
"\n",
"A_lamb = jit(lambdify((q1, q2, q3, q4, q5, q6, q7), A, 'numpy'))\n",
"J_lamb = jit(lambdify((q1, q2, q3, q4, q5, q6, q7), J, 'numpy'))"
],
"id": "c7378849",
"execution_count": 6,
"outputs": []
},
{
"cell_type": "markdown",
"metadata": {
"id": "c2e7fd9b"
},
"source": [
"## IIK Implementation\n",
"\n",
"We are now ready to implement the actual IIK."
],
"id": "c2e7fd9b"
},
{
"cell_type": "code",
"metadata": {
"id": "7ad80c11"
},
"source": [
"@jit\n",
"def incremental_ik(q, A, A_final, step=0.1, atol=1e-4):\n",
" while True:\n",
" delta_A = (A_final - A)\n",
" if np.max(np.abs(delta_A)) <= atol:\n",
" break\n",
" J_q = J_lamb(q[0,0], q[1,0], q[2,0], q[3,0], q[4,0], q[5,0], q[6,0])\n",
" J_q = J_q / np.linalg.norm(J_q) # normalize Jacobian\n",
" \n",
" # multiply by step to interpolate between current and target pose\n",
" delta_q = np.linalg.pinv(J_q) @ (delta_A*step)\n",
" \n",
" q = q + delta_q\n",
" A = A_lamb(q[0,0], q[1,0],q[2,0],q[3,0],q[4,0],q[5,0],q[6,0])\n",
" return q, np.max(np.abs(delta_A))"
],
"id": "7ad80c11",
"execution_count": 7,
"outputs": []
},
{
"cell_type": "code",
"metadata": {
"colab": {
"base_uri": "https://localhost:8080/"
},
"id": "ae2bc32d",
"outputId": "a379fd92-db14-49d5-e675-91db5b27a71e"
},
"source": [
"# define joint limits for the Panda robot\n",
"limits = [\n",
" (-2.8973, 2.8973),\n",
" (-1.7628, 1.7628),\n",
" (-2.8973, 2.8973),\n",
" (-3.0718, -0.0698),\n",
" (-2.8973, 2.8973),\n",
" (-0.0175, 3.7525),\n",
" (-2.8973, 2.8973)\n",
"]\n",
"\n",
"# create initial pose\n",
"q_init = np.array([l+(u-l)/2 for l, u in limits], dtype=np.float64).reshape(7, 1)\n",
"A_init = A_lamb(*(q_init.flatten()))\n",
"print(A_init.reshape(3, 4, order='F'))"
],
"id": "ae2bc32d",
"execution_count": 8,
"outputs": [
{
"output_type": "stream",
"text": [
"[[ 0.9563065 0. 0.29236599 0.58193844]\n",
" [ 0. -1. 0. 0. ]\n",
" [ 0.29236599 -0. -0.9563065 0.654902 ]]\n"
],
"name": "stdout"
}
]
},
{
"cell_type": "code",
"metadata": {
"colab": {
"base_uri": "https://localhost:8080/"
},
"id": "c24599a8",
"outputId": "24ed80a7-c8cf-43d3-fe38-d79933f9f6db"
},
"source": [
"# generate random final pose within joint limits\n",
"\n",
"np.random.seed(0)\n",
"\n",
"q_rand = np.array([np.random.uniform(l, u) for l, u in limits], dtype=np.float64).reshape(7, 1)\n",
"A_final = A_lamb(*(q_rand).flatten())\n",
"print(A_final.reshape(3, 4, order='F'))"
],
"id": "c24599a8",
"execution_count": 9,
"outputs": [
{
"output_type": "stream",
"text": [
"[[ 0.21582232 0.95638546 0.19684405 0.55544547]\n",
" [ 0.91784019 -0.26748917 0.29328985 0.50407938]\n",
" [ 0.3331518 0.11737288 -0.93553914 0.33267676]]\n"
],
"name": "stdout"
}
]
},
{
"cell_type": "code",
"metadata": {
"colab": {
"base_uri": "https://localhost:8080/"
},
"id": "bad82d63",
"outputId": "9c06c29e-9d20-456f-adb7-dd86ad9951be"
},
"source": [
"q_final, _ = incremental_ik(q_init, A_init, A_final, atol=1e-6)\n",
"q_final.flatten()"
],
"id": "bad82d63",
"execution_count": 10,
"outputs": [
{
"output_type": "execute_result",
"data": {
"text/plain": [
"array([ 0.44462097, 0.69891914, 0.37442864, -1.44809794, -0.21841531,\n",
" 2.45359754, -0.47850648])"
]
},
"metadata": {
"tags": []
},
"execution_count": 10
}
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "a3823360"
},
"source": [
"## Test Runtime and Convergence Robustness\n",
"\n",
"Randomly sample 10k target poses within joint limits to test runtime and convergence robustness."
],
"id": "a3823360"
},
{
"cell_type": "code",
"metadata": {
"colab": {
"base_uri": "https://localhost:8080/"
},
"id": "463ec2f2",
"outputId": "6084f280-245f-49f9-c4b5-83a1bb484671"
},
"source": [
"%%timeit -n10000\n",
"q_rand = np.array([np.random.uniform(l, u) for l, u in limits], dtype=np.float64).reshape(7, 1)\n",
"incremental_ik(q_rand, A_init, A_final)"
],
"id": "463ec2f2",
"execution_count": 14,
"outputs": [
{
"output_type": "stream",
"text": [
"10000 loops, best of 5: 1.12 ms per loop\n"
],
"name": "stdout"
}
]
},
{
"cell_type": "code",
"metadata": {
"id": "2773c0f0"
},
"source": [
""
],
"id": "2773c0f0",
"execution_count": 12,
"outputs": []
}
]
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment