{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# About this notebook"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's learn the forward kinematics and the inverse kinematics of the robot arm by moving our hands. We will explain how to construct forward kinematics using trigonometric functions and how to estimate joint angle using Newton's method."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Written by Yosuke Matsusaka (MID Academic Promotions, Inc. [@yosuke](https://twitter.com/yosuke))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This note is published under Creative Commons BY-SA license. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Preparation"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This note is a hands-on type teaching material using the ipython notebook. On Ubuntu type following commands:\n",
"```\n",
"sudo apt-get install imagemagick asymptote graphviz\n",
"sudo pip3 install jupyter matplotlib sympy graphviz\n",
"jupyter notebook\n",
"```\n",
"to launch the environment."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Once you have launched the ipython notebook, please create a new notebook, and read the library for graph drawing and symbolic math processing as follows:"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"import matplotlib.animation as animation\n",
"from IPython.display import Image\n",
"from sympy import *\n",
"init_printing()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Please enter the following code as well. This part will give us a magic of displaying animated GIF."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"# Part of code is taken from: http://nbviewer.ipython.org/url/jakevdp.github.io/downloads/notebooks/AnimationEmbedding.ipynb\n",
"from tempfile import NamedTemporaryFile\n",
"import base64\n",
"\n",
"GIF_TAG = \"\"\"\"\"\"\n",
"\n",
"def anim_to_html(anim):\n",
" if not hasattr(anim, '_encoded_video'):\n",
" with NamedTemporaryFile(suffix='.gif') as f:\n",
" anim.save(f.name, writer='imagemagick', fps=4)\n",
" video = open(f.name, \"rb\").read()\n",
" anim._encoded_video = base64.b64encode(video).decode('utf-8')\n",
" return GIF_TAG.format(anim._encoded_video)\n",
"\n",
"animation.Animation._repr_html_ = anim_to_html"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here, we will initialize the graph area. We will confirm the operation of the robot arm on this graph."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"# Part of code is taken from: http://studywolf.wordpress.com/2013/09/20/python-visualization-with-matplotlib/\n",
"fig = plt.figure(figsize=(4,4))\n",
"ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,\n",
" xlim=(-1, 1), ylim=(-1, 1))\n",
"ax.grid()\n",
"line, = ax.plot([], [], 'o-', lw=4, mew=5)\n",
"infoline = ax.text(0.02, 0.95, '', transform=ax.transAxes)\n",
"\n",
"def init():\n",
" line.set_data([], [])\n",
" infoline.set_text('')\n",
" return line, infoline"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The following is plotting library to draw description figures (installation is optional)."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"import graphviz"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"import subprocess # to run asymptote\n",
"import shlex"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's get started."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Forward kinematics of the robot arm"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The term \"forward kinematics\" means the task to estimate the position of the robot hand when each joint angle of the robot arm is provided."
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n",
"\n",
"\n",
"\n"
],
"text/plain": [
""
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"graphviz.Source('digraph G { rankdir=LR; joint_angle -> hand_position [label=\"estimate\"] }')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"People tends to think difficultly, because it contains the word \"kinematics\". However, the task is not so complex as described as the follows.\n",
"\n",
"Calculate the forward kinematics, we need to know the structure of the robot arm. For this specific notebook we will select PA10-7C robot arm made by Mitsubishi Heavy Industries. Technical information of the arm has been published on the following pages.\n",
"\n",
"https://www.mhi.co.jp/products/detail/pa10_6c_7c.html\n",
"\n",
"The length of the links and joints of the robot arm are described in the following figure.\n",
"\n",
"https://www.mhi.co.jp/products/detail/images/7c_mass.jpg"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"PA10 is the robot arm with 7 degree-of-freedom (7 joints). However for this specific notebook, we will take into account only the 2 degree-of-freedom (2 joints) on up and down direction to greatly simplify the model.\n",
"\n",
"From Mitsubishi Heavy Industry's technical information, length between the first and the second joint from the root of PA10 is 450mm, length between the second joint and the hand is 480mm. By using this information, let's try to estimate the position of the hand given the angle of the first and the second joint."
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAAwAAAASCAYAAABvqT8MAAAABHNCSVQICAgIfAhkiAAAAN1JREFU\nKJHN0r0uRFEUxfGfyRgFxkc0U4gC0VNotd6BB/AAGr1M6z2UovUKJFQ6ImRoiExjIqO4+ybHzZ5C\nZzcre53z3/skZ/HHmmr0W+jjAWOs4BiDDF7AEw4K7wR3mMmAU7yiXXjLGOEoA+5xkfi3uKqbVug8\nNvGYAM/YaQJroR8JMEQXnRLoho4mALBYAt+h4wSYDm2XwFtysa7Z0M8SGMT0pQnAexMY4hqrCbCB\nm7ppFQeX2PU7Lusx5Dx7ay9WHxbemerjOrVRxuAFe6qIbGNOFY19fGUb/kn9ABQxJaUniGJOAAAA\nAElFTkSuQmCC\n",
"text/latex": [
"$$0$$"
],
"text/plain": [
"0"
]
},
"execution_count": 7,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"f=open('/tmp/pa10-simplified.asy', 'w')\n",
"f.write('''\n",
"import math; import graph; import geometry;\n",
"size(12cm); xaxis(\"$x$\"); yaxis(\"$y$\");\n",
"real theta1=pi/4; real theta2=pi/3;\n",
"real L1=0.45; real L2=0.48;\n",
"pair l1p = (L1*sin(theta1), L1*cos(theta1));\n",
"pair l2p = l1p + (L2*sin(theta1+theta2), L2*cos(theta1+theta2));\n",
"draw((0,0)--l1p--l2p); draw(l1p--l1p*1.5, dashed); dot(l2p);\n",
"label(\"$J1(0,0)$\", (0,0), NW); label(\"$J2(u,v)$\", l1p, NW); label(\"$H(x,y)$\", l2p, NE);\n",
"draw(\"$L1=450mm$\",(0,0)-0.02*I*l1p--l1p-0.02*I*l1p, red, Arrows, Bars, PenMargins);\n",
"draw(\"$L2=480mm$\", l1p-0.02*I*l2p--l2p-0.02*I*l2p, red, Arrows, Bars, PenMargins);\n",
"draw(\"$\\theta_1$\", arc((0,1),(0,0),l1p,0.1), blue, PenMargins);\n",
"draw(\"$\\theta_2$\", arc(l1p,0.1,degrees(l1p),degrees(l2p-l1p),CW), blue, PenMargins);\n",
"''')\n",
"f.close()\n",
"subprocess.call(shlex.split('asy /tmp/pa10-simplified.asy -f png -o pa10-simplified.png'))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
""
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"First we define the angle of each joint as a variable."
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"t1 = Symbol('theta_1')\n",
"t2 = Symbol('theta_2')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We define the distance between each joint as a constant."
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"L1 = Symbol('L_1')\n",
"L2 = Symbol('L_2')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Before estimating the position of the hand, We first estimate the position of the second joint. Position of the second joint will be calculated in the following manner using the trigonometric functions."
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAMEAAAAXCAYAAACyJSuuAAAABHNCSVQICAgIfAhkiAAABddJREFU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\nBgwYMOD/lHcAZjaVSPjdAKUAAAAASUVORK5CYII=\n",
"text/latex": [
"$$\\left [ L_{1} \\sin{\\left (\\theta_{1} \\right )}, \\quad L_{1} \\cos{\\left (\\theta_{1} \\right )}\\right ]$$"
],
"text/plain": [
"[L₁⋅sin(θ₁), L₁⋅cos(θ₁)]"
]
},
"execution_count": 10,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"u = L1 * sin(t1)\n",
"v = L1 * cos(t1)\n",
"[u, v]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Then we estimate the position of the hand. We add the distance to the hand to the position of the second joint."
]
},
{
"cell_type": "code",
"execution_count": 11,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAdsAAAAXCAYAAABDJ+VPAAAABHNCSVQICAgIfAhkiAAACNtJREFU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\nQmCC\n",
"text/latex": [
"$$\\left [ L_{1} \\sin{\\left (\\theta_{1} \\right )} + L_{2} \\sin{\\left (\\theta_{1} + \\theta_{2} \\right )}, \\quad L_{1} \\cos{\\left (\\theta_{1} \\right )} + L_{2} \\cos{\\left (\\theta_{1} + \\theta_{2} \\right )}\\right ]$$"
],
"text/plain": [
"[L₁⋅sin(θ₁) + L₂⋅sin(θ₁ + θ₂), L₁⋅cos(θ₁) + L₂⋅cos(θ₁ + θ₂)]"
]
},
"execution_count": 11,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"x = u + L2 * sin(t1+t2)\n",
"y = v + L2 * cos(t1+t2)\n",
"[x, y]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now the hand the position is estimated. We will confirm the result by drawing a graph."
]
},
{
"cell_type": "code",
"execution_count": 12,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
""
],
"text/plain": [
""
]
},
"execution_count": 12,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"import math\n",
"\n",
"subs = {\n",
" L1: 450.0 / 1000.0,\n",
" L2: 480.0 / 1000.0,\n",
" t1: 0.0,\n",
" t2: 0.0\n",
"}\n",
"\n",
"def animate(i):\n",
" if i < 10:\n",
" subs[t1] = subs[t1] + math.pi / 30.0\n",
" elif i < 20:\n",
" subs[t2] = subs[t2] + math.pi / 30.0\n",
" else:\n",
" subs[t1] = subs[t1] - math.pi / 30.0\n",
" subs[t2] = subs[t2] - math.pi / 30.0\n",
" line.set_data([0, u.evalf(subs=subs), x.evalf(subs=subs)],\n",
" [0, v.evalf(subs=subs), y.evalf(subs=subs)])\n",
" infoline.set_text('t1 = %.2f, t2 = %.2f' % (subs[t1], subs[t2]))\n",
" return line, infoline\n",
"\n",
"animation.FuncAnimation(fig, animate, frames=30,\n",
" interval=25, blit=True, \n",
" init_func=init)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Seems there is no problem. Forward kinematics is now being solved."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Inverse kinematics of the robot arm"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The term \"inverse kinematics\" stands for the task to estimate the angle of each joint from the position of the hand. Just as the reverse of the forward kinematics."
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n",
"\n",
"\n",
"\n"
],
"text/plain": [
""
]
},
"execution_count": 13,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"graphviz.Source('digraph G { rankdir=LR; hand_position -> joint_angle [label=\"estimate\"] }')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"It is necessary to calculate the inverse kinematics for the operations such as pick-and-place manipulating the robot arm freely. It will be somewhat more difficult than the forward kinematics, but it is yet not so difficult as you imagine from the word \"kinematics\".\n",
"\n",
"There are two ways to calculate the inverse kinematics, one is analytical method and the other is numerical method."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## How to calculate inverse kinematics analytically"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Since the forward kinematics is already given, what we have to do is to seek the opposite. In mathematical word, we have to find an inverse function. Analytical method tries to find such inverse function in a mathematical sense."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Forward kinematics is finally expressed as the following two equations."
]
},
{
"cell_type": "code",
"execution_count": 14,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAdsAAAAXCAYAAABDJ+VPAAAABHNCSVQICAgIfAhkiAAACNtJREFU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\nQmCC\n",
"text/latex": [
"$$\\left [ L_{1} \\sin{\\left (\\theta_{1} \\right )} + L_{2} \\sin{\\left (\\theta_{1} + \\theta_{2} \\right )}, \\quad L_{1} \\cos{\\left (\\theta_{1} \\right )} + L_{2} \\cos{\\left (\\theta_{1} + \\theta_{2} \\right )}\\right ]$$"
],
"text/plain": [
"[L₁⋅sin(θ₁) + L₂⋅sin(θ₁ + θ₂), L₁⋅cos(θ₁) + L₂⋅cos(θ₁ + θ₂)]"
]
},
"execution_count": 14,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"[x, y]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In the analytical method, we will transform combine the expression like a puzzle with trial and error.\n",
"\n",
"When converting the trigonometric functions, it have been known that it sometimes become better organized by summing the squared equations."
]
},
{
"cell_type": "code",
"execution_count": 15,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAA8UAAAAbCAYAAABV7i2uAAAABHNCSVQICAgIfAhkiAAADahJREFU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\nRVEURVEURVEURVGUjvF/W15Wcgxa40oAAAAASUVORK5CYII=\n",
"text/latex": [
"$$L_{1}^{2} \\sin^{2}{\\left (\\theta_{1} \\right )} + L_{1}^{2} \\cos^{2}{\\left (\\theta_{1} \\right )} + 2 L_{1} L_{2} \\sin{\\left (\\theta_{1} \\right )} \\sin{\\left (\\theta_{1} + \\theta_{2} \\right )} + 2 L_{1} L_{2} \\cos{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{1} + \\theta_{2} \\right )} + L_{2}^{2} \\sin^{2}{\\left (\\theta_{1} + \\theta_{2} \\right )} + L_{2}^{2} \\cos^{2}{\\left (\\theta_{1} + \\theta_{2} \\right )}$$"
],
"text/plain": [
" 2 2 2 2 \n",
"L₁ ⋅sin (θ₁) + L₁ ⋅cos (θ₁) + 2⋅L₁⋅L₂⋅sin(θ₁)⋅sin(θ₁ + θ₂) + 2⋅L₁⋅L₂⋅cos(θ₁)⋅c\n",
"\n",
" 2 2 2 2 \n",
"os(θ₁ + θ₂) + L₂ ⋅sin (θ₁ + θ₂) + L₂ ⋅cos (θ₁ + θ₂)"
]
},
"execution_count": 15,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"i = expand(x**2 + y**2)\n",
"i"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This formula looks still complicated, but it can be greatly simplified by using the each theorem of trigonometric function shown below.\n",
"\n",
"Pythagorean theorem: $ cos^2 \\theta + sin^2 \\theta = 1 $\n",
"\n",
"Addition theorem: $ cos(\\alpha - \\beta) = cos(\\alpha) cos(\\beta) + sin(\\alpha) sin(\\beta) $\n",
"\n",
"Based on these knowledge, we will use sympy to organize the equation automatically (yes, I am lazy)."
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAM0AAAAbCAYAAADf2wtbAAAABHNCSVQICAgIfAhkiAAABqtJREFU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\nawAAAABJRU5ErkJggg==\n",
"text/latex": [
"$$L_{1}^{2} + 2 L_{1} L_{2} \\cos{\\left (\\theta_{2} \\right )} + L_{2}^{2}$$"
],
"text/plain": [
" 2 2\n",
"L₁ + 2⋅L₁⋅L₂⋅cos(θ₂) + L₂ "
]
},
"execution_count": 16,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"j = simplify(i)\n",
"j"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"After the simplification, $\\theta_1$ disappears (such thing will not always happen, but we are lucky in this case).\n",
"\n",
"Calculation of the inverse function becomes easy after we converted to a simple formula. $\\theta_2$ will be solved by using the $cos$ inverse function $acos$ as the follows. Here we get two solutions, we will discuss about this later."
]
},
{
"cell_type": "code",
"execution_count": 17,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAArYAAAAhCAYAAAAs/+g+AAAABHNCSVQICAgIfAhkiAAADtVJREFU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\n",
"text/latex": [
"$$\\left [ - \\operatorname{acos}{\\left (- \\frac{1}{2 L_{1} L_{2}} \\left(L_{1}^{2} + L_{2}^{2} - x^{2} - y^{2}\\right) \\right )} + 2 \\pi, \\quad \\operatorname{acos}{\\left (\\frac{1}{2 L_{1} L_{2}} \\left(- L_{1}^{2} - L_{2}^{2} + x^{2} + y^{2}\\right) \\right )}\\right ]$$"
],
"text/plain": [
"⎡ ⎛ ⎛ 2 2 2 2⎞ ⎞ ⎛ 2 2 2 2⎞⎤\n",
"⎢ ⎜-⎝L₁ + L₂ - x - y ⎠ ⎟ ⎜- L₁ - L₂ + x + y ⎟⎥\n",
"⎢- acos⎜───────────────────────⎟ + 2⋅π, acos⎜─────────────────────⎟⎥\n",
"⎣ ⎝ 2⋅L₁⋅L₂ ⎠ ⎝ 2⋅L₁⋅L₂ ⎠⎦"
]
},
"execution_count": 17,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"from sympy.solvers import solve\n",
"x2 = Symbol('x')\n",
"y2 = Symbol('y')\n",
"j2 = (x2**2 + y2**2)\n",
"k = solve(j2 - j, t2)\n",
"k"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"> FYI note:\n",
"> The sympy \"solve\" function provides us a high-performance solving algorithm. Why don't we solve the inverse kinematics directly by using this function? However, it can not be solved automatically given the forward kinematics equation of the original form. This not is because sympy is low-performance, but the conversion necessary to solve inverse kinematics is fairly complex. At the end of this notebook, we have information on the recently developed powerful libraries such as IKFast which has a function to express the kinematic structure of the robot in easily solvable form and solve inverse kinematics automatically by applying several conversion techniques specialized for kinematics."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We got $\\theta_2$ already, so let's calculate $\\theta_1$. Here, we solve by using the law of cosines.\n",
"\n",
"First, we draw the auxiliary line from the first joint to the hand, and let the angle formed by the auxiliary line and the $x$ axis as $\\alpha$."
]
},
{
"cell_type": "code",
"execution_count": 18,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAAwAAAASCAYAAABvqT8MAAAABHNCSVQICAgIfAhkiAAAAN1JREFU\nKJHN0r0uRFEUxfGfyRgFxkc0U4gC0VNotd6BB/AAGr1M6z2UovUKJFQ6ImRoiExjIqO4+ybHzZ5C\nZzcre53z3/skZ/HHmmr0W+jjAWOs4BiDDF7AEw4K7wR3mMmAU7yiXXjLGOEoA+5xkfi3uKqbVug8\nNvGYAM/YaQJroR8JMEQXnRLoho4mALBYAt+h4wSYDm2XwFtysa7Z0M8SGMT0pQnAexMY4hqrCbCB\nm7ppFQeX2PU7Lusx5Dx7ay9WHxbemerjOrVRxuAFe6qIbGNOFY19fGUb/kn9ABQxJaUniGJOAAAA\nAElFTkSuQmCC\n",
"text/latex": [
"$$0$$"
],
"text/plain": [
"0"
]
},
"execution_count": 18,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"f=open('/tmp/pa10-firstjoint.asy', 'w')\n",
"f.write('''\n",
"import math; import graph; import geometry;\n",
"size(12cm); xaxis(\"$x$\"); yaxis(\"$y$\");\n",
"real theta1=pi/4; real theta2=pi/3;\n",
"real L1=0.45; real L2=0.48;\n",
"pair l1p = (L1*sin(theta1), L1*cos(theta1));\n",
"pair l2p = l1p + (L2*sin(theta1+theta2), L2*cos(theta1+theta2));\n",
"draw((0,0)--l1p--l2p); draw(l1p--l1p*1.5, dashed); dot(l2p); draw((0,0)--l2p, dashed); \n",
"label(\"$J1(0,0)$\", (0,0), NW); label(\"$J2(u,v)$\", l1p, NW); label(\"$H(x,y)$\", l2p, NE);\n",
"draw(\"$L1$\",(0,0)-0.02*I*l1p--l1p-0.02*I*l1p, red, Arrows, Bars, PenMargins);\n",
"draw(\"$L2$\", l1p-0.02*I*l2p--l2p-0.02*I*l2p, red, Arrows, Bars, PenMargins);\n",
"draw(\"$\\\\theta_1$\", arc((0,1),(0,0),l1p,0.1), blue, PenMargins);\n",
"draw(\"$\\\\theta_2$\", arc(l1p,0.1,degrees(l1p),degrees(l2p-l1p),CW), blue, PenMargins);\n",
"draw(\"$\\\\alpha$\", arc((1,0),(0,0),l2p,0.2), blue, PenMargins);\n",
"''')\n",
"f.close()\n",
"subprocess.call(shlex.split('asy /tmp/pa10-firstjoint.asy -f png -o pa10-firstjoint.png'))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
""
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$\\alpha$ can be expressed by using $atan$ as the follows."
]
},
{
"cell_type": "code",
"execution_count": 19,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAFgAAAAZCAYAAAC1ken9AAAABHNCSVQICAgIfAhkiAAABFlJREFU\naIHt2Xus13Mcx/FHHUlqOtSIWLpRmnJZ5lJZdCSGpaxmaC5blj8ct41lTBtGuZY2/4g1yghbblvu\nlcsyl9zGUIxmU5RhFPLH+/Nd3/M933Oc4/etZOe5tb6/z+X1fX/en8v7/fkeOuigFbpUqHV2DX1v\nwpFVGfJf4RxcVJHWoXiqhv57YBkGVmPOzmcs7q1Qbxam1KgxGCtRV7s5wUCsQ7+qBNtIT6zCnhVq\nvoNuFegsxvkV6IBG/Ca2x47ktvTuqjgGCyvSOhYfV6RlCV6tSqyNdMcP6FWh5j2YUJFWHX7C4VlB\n5xrERuG1Wi1qJ6djLTZUpFcnzvNlFen9iddxalawW0mjkzADX2JvsUqnY7QIBBem8n2Tcc9jKe5r\no0bGWWjAcEzDPpiMTmLybsGzBdsa0gDKGIArcTAeSf8yrsJITC0Z63L8UaJ3NM7D1qR5SRpDPfri\nRnxR0u8jHNGCjS7G+iQoCf2CdwvtpuN35YGhLRq74+70vAorhBM6pbJr8XWJ9tvp3WXMT7qXY3Wh\n7n08WtJnAU4oKR+MuTl7HsRnOD61/yvZW8b1eLOsYji2iFnL8w3uLJQtFk75txoNtq3WDeI8z3Od\nmJQi65WnU6Nwbnp+Do/n6urF1p1R6NMV79nmxDzzxXmf8RjeSs8HYQ56l/ST3vNJWcXT+FHT21E/\nsUXOLLT9DjfXoLG/WP0jUt2ogs5i5UfBFowvKe8jHNZXbPeJuboz0juGFfpMwq0lWpnNeb5VPt4y\nLhWrvQn1ybDiNpomZr8+VzZMGHxKDRoZjWKl7p4r64KNYhUX2az1iH8FNglnZ9yO7zVfqUvkon0r\nDBXjHdeGtjBTHHvYlkUMEhH1jULjE8U22oj+qWyscGS2wurFtmmPRsZYcfvZnCubgL2wKOnlV9NG\nEQxbYjxeFvEhY4zIdrbmynriQHzQilbGycm+/I4a0Er7XliT/cgcvCn9nw8sXZN4lutmh/po4bCf\n0+9GsULbo5G9e4xwSJ4L8JJIx8bhqFzdGq07uB8+zf3unvoX8/VJeLIFjW5i1Weru0EEzV9zdl/d\nig2HyAX0zMGfC6dlM1OHeeghUpHeIsHP6tam55HpxevaqUEMvB6vlBi4VKSQU8S5nrECh7UyuK80\nnYDZ4sgpOniq2CFlnCYcOAxD0njyO2Kmlm9+nUU8eaGscpAYzF0iRRkqIvOLeAD7pXYjxHa5A9do\nellpqwYR9FZrnotPEfnv/Zo7s0HkmS0xREzCPBHpl4tJzdvYR/NJzdM72To7aXTHQ8meucmGlhgp\nFtouS1eR1vVtQ9vOIttZUChv1Dxlq4qFuGw7ae8wZokzssgiTS8YE8XHqMGFdiu1nMPWQv/0/ir/\nCLBT6IEPNQ9268UWhgPENb14NR6EZ7aDTV3EJ4MWr8i7GsfhCU3P1sliZc/Bw+LTYZEbNL9lVsEs\n8V3jf8UEcbFoD0vFDqiaso9mHXTQQQf/xN+mte7BnesQFQAAAABJRU5ErkJggg==\n",
"text/latex": [
"$$\\operatorname{atan}{\\left (\\frac{y}{x} \\right )}$$"
],
"text/plain": [
" ⎛y⎞\n",
"atan⎜─⎟\n",
" ⎝x⎠"
]
},
"execution_count": 19,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"alpha = atan(y2/x2)\n",
"alpha"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Further"
]
},
{
"cell_type": "code",
"execution_count": 20,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAMYAAAAZCAYAAABq81GnAAAABHNCSVQICAgIfAhkiAAABx9JREFU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\nHlF4AAAAAElFTkSuQmCC\n",
"text/latex": [
"$$- \\theta_{1} - \\operatorname{atan}{\\left (\\frac{y}{x} \\right )} + \\frac{\\pi}{2}$$"
],
"text/plain": [
" ⎛y⎞ π\n",
"-θ₁ - atan⎜─⎟ + ─\n",
" ⎝x⎠ 2"
]
},
"execution_count": 20,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"beta = pi / 2 - t1 - alpha\n",
"beta"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Assuming a corner $\\beta$ described as the above, we can draw following triangle with a focus on the second joint."
]
},
{
"cell_type": "code",
"execution_count": 21,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAAwAAAASCAYAAABvqT8MAAAABHNCSVQICAgIfAhkiAAAAN1JREFU\nKJHN0r0uRFEUxfGfyRgFxkc0U4gC0VNotd6BB/AAGr1M6z2UovUKJFQ6ImRoiExjIqO4+ybHzZ5C\nZzcre53z3/skZ/HHmmr0W+jjAWOs4BiDDF7AEw4K7wR3mMmAU7yiXXjLGOEoA+5xkfi3uKqbVug8\nNvGYAM/YaQJroR8JMEQXnRLoho4mALBYAt+h4wSYDm2XwFtysa7Z0M8SGMT0pQnAexMY4hqrCbCB\nm7ppFQeX2PU7Lusx5Dx7ay9WHxbemerjOrVRxuAFe6qIbGNOFY19fGUb/kn9ABQxJaUniGJOAAAA\nAElFTkSuQmCC\n",
"text/latex": [
"$$0$$"
],
"text/plain": [
"0"
]
},
"execution_count": 21,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"f=open('/tmp/pa10-secondjoint.asy', 'w')\n",
"f.write('''\n",
"import math; import graph; import geometry;\n",
"size(12cm); xaxis(\"$x$\"); yaxis(\"$y$\");\n",
"real theta1=pi*0.327; real theta2=pi/3;\n",
"real L1=0.45; real L2=0.48;\n",
"pair l1p = (L1*sin(theta1), L1*cos(theta1));\n",
"pair l2p = l1p + (L2*sin(theta1+theta2), L2*cos(theta1+theta2));\n",
"draw((0,0)--l1p--l2p); draw(l1p--l1p*1.5, dashed);\n",
"label(\"$J1(0,0)$\", (0,0), NW); label(\"$J2(u,v)$\", l1p, NW); label(\"$H(x,y)$\", l2p, NE);\n",
"draw(\"$L1$\",(0,0)-0.02*I*l1p--l1p-0.02*I*l1p, red, Arrows, Bars, PenMargins);\n",
"draw(\"$L2$\", l1p-0.02*I*l2p--l2p-0.02*I*l2p, red, Arrows, Bars, PenMargins);\n",
"draw(\"$\\\\theta_2$\", arc(l1p,0.1,degrees(l1p),degrees(l2p-l1p),CW), blue, PenMargins);\n",
"draw(\"$\\\\beta$\", arc((1,0),(0,0),l1p,0.15), blue, PenMargins);\n",
"''')\n",
"f.close()\n",
"subprocess.call(shlex.split('asy /tmp/pa10-secondjoint.asy -f png -o pa10-secondjoint.png'))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
""
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Between $\\beta$ and $\\theta_2$ following relationship should exist from cosine theorem (the same way of thinking when decompose the triangle to derive the theorem)."
]
},
{
"cell_type": "code",
"execution_count": 22,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAJYAAAAlCAYAAACgXxA5AAAABHNCSVQICAgIfAhkiAAACAtJREFU\neJzt3H+wVVUVwPHPewKG6QvLQhIKkOwHlBoZBRKBj0ICFY0Q8UdJSU3ThDn20/KZYxET5lQz5TTT\nONM04hhFiZTZj5dSjaaWUmmKZRIoE1OAJVkk/bHO4Z57373v/nj33Mej+525c87dZ9+193ln3b3W\nXmvdR5s2bSqyDJ8a7En8n9CZvA55zsRN6GiizGNwJf6DDfgERg9A3n14ZwOfG5k5fy4+mcj5TJXP\nvQTvaGC8WpiIm3FETvIPCibgd3heDrJfiv0Y2wRZC3FcnZ8Zi7cn5x24FVOT9zfihCqfX4xT6xyz\nVqbj+xiWk/xBpxeLcpL9bjyUk+xauDpzvhw/yLz/LuZX+XwnbmjynLJ8HJ/PUf6gsQj35ih/Lb5c\n52dG40NitThXPNhu3I2PJH0W4rfoEWbtInxHmLqUeTgrOe/AlkRmyuOYUcN8rsexdd5DrQzD/ZhZ\n6eJQpUd1X6NROjAHKypc78T7FXyg1cnxAvxd+CDQhR9hVqbvLZiGU8Q9wDl4K76dvF+koIhThVme\njOPxfLxQPNRK80jZgpOF2Wo2+8T8r8PrhNtwgKHq3c/AeGES8uAk8QB/WtL+yuS4AOvFg5ym4Pvc\niqvECnUtNibt/y2Rs0849Cl/E0qYMg67kvPXJ/J6sApPitX0H/3MI2VXch95sR5HJvMoYqgq1oX4\nHv6Vk/xuYWZ3lbSn/txELE3OHxWKQOwiJ+Dy5LwXIyqMUapsWZ6TOe9SUMIO4W9dV2UeKUeJFTQv\n9uMbYtUsYiCKdTy2i2W61czHj3OU3y1MWJZLhW8EX0le8GqxohDxtMn4mTBl9ytWklrJKt2jYnWC\nd2FdIre/eaSMF7vmPPkW5grzfICB+FgLxTK7YwAyGuHlYit+Zw6yp+Fs4RPtxhXCqZ6BSSI+BM8k\nrxlCibYn7XtFXG2K8Hs2ii3/AjwrlLVLwXTcjaMTOWPxMH6RHMfgCeHYz8V7ROzoysx8K80j5Wj8\nuaG/RO08JMzz2zRpF7pO3EyrWSa+wc0MiDZCl1C8PJglVqeBzOMt4svfCm7WxNDGDsWxllbxOdwz\nCOOWcgmGJ6/uHOSvwmENzuNFSXuruAqbq3WaI+zmanwN5yuYnSUiUHeXcNw2Je9Lnbf+ZKScKeJE\ndwiHdyo+K/6gm1QOAN4kv91grSzBHuwUzvGUHMYYJcIXjcyj1emW88VmpaLPvlxMcnzy/jj8E78u\n6bdC2PaR+lKLjBEKO5tfCUW6TMG8fVQEAcuxSShrm4OHbrHQjEkbshr2GnwVK/FY0rZNfBNK4zmz\nhULsLWmvVcYsoSAdYsu8A2sUgmwdeEGFm+jCUxWutRkcdibHson6DUIBhmfa0kTsGSV9n8Q1A5Ax\nRqx2JybXSpOla8XuqByPCJPZ5uDhFeI5Tk8b0hVrlMhP/VDYypQ3i23yHZm2yUIzS3eE9ch4Qqx2\ns/G04vjL8ETOLTXeVJvB59/J8fC0IY1jTRI7kF+WfGAWfiMi0BPwJ6EM+xRWlFEiwju6Dhkps/Hz\nzMTgdGHubkzkjVUci9mbvYES9ldob9M8yoV50ufxTNqQrli7k+PjJZ1PU1iZLkuOM4WipNHglSJS\nXI+MdOw36eu/XYifCB+tG68tub5b5KfK0dF+5f4qR5pdeDptSBVri1CWicn7w0Qo4EiRUjhGJErT\na48l56ckwrbXKYNQmFEin5blBGEGh4nt9IaS69tEnKbNwUP6PMpmYSaJh/gFfElk8s8TObmvK3j8\nJwozuEYkWzsbkEE48w/om1ZaIlIh1+NVZea5St+cWCnNLi2uh8Ecu5TScFCt5c31ljZfJNyZoVrU\ngFDUPTX0a2ZpMcWFdq0euxGypc3UX95cT2nzNWKROMBQ1LB7xGZhfJV+c/EH/KVJ406uo2+zx26E\nFSL7kXKxcE/SqtsjhIWpxDpRnl0LkxXXlw3JCtKHxQZhpoKvV45ypS+totGx3yhW5AfEqncnbhOb\nnDnCV50oNkO9wsQuw1ahNPOEmZunuGy7Ax9LXiknK2y4yvGs2OUdK+KWlehI5n15tnEoKhbhg80R\nRWblqFZaTKzWa0SdVTNptKx5jChNnio2Qx8W1aN/TPq8ISP/PmGqztK3FJri0mYqlzdvxgfKzCWl\nltLmKcKv3JhtHKqKdYNIfr9XJnaSob/S4gfFz8WWixhbOUaLMEp2e32q4qK9p5TPPlQbOy0n3irM\nzVSxupwjVuK0pip9yJ/G7zNy9gsze4HIUNwulHiTKIemuLSZ4vJm4gcfa0XwutxcUmopbV4sVtWd\n2cahqlh3iW/yGQrf1iz9lRY/KEzAtfqmqlJ2KDYbxEPpqWFu1caeKFIgqxXKie9V+RfGR+m7uxsu\nEvlpKfR0UQ3SK3bSpVWr5cqbzxMB6nJzyY69rZ977RSm+IP99BlyLFC5Lus2fVeTS/VVpN46xuup\nsV+1sQ8XD4wwMS9Ozsfhr0JRUs4V8b7NCkrXKfzMKSKscVKm/zqhRLeXjL9YIb96sVgF+5tLyhf1\nX3q+VBQj9AmcDtUVi4gRrRR+xvqkrdbS4jwYaFnz1uReekSccL8wp4+IjMUVwokeh/eJ+vv5+pZC\n71Fc2kzl8uaBlDaPEDGxSxyCqbRxYgfVVa1jBXrr6Lu0epeaybOsmdaUNl8tqnkPWU7HNzVWA9/b\n3KnUTN5lzeRb2nyaQ/x/N6ScLVIntTJSmNEdYodUrhI2L1pR1kx+pc0vE75cK/9mbdq0adOmTZs2\nbfA/rljpR6CxToAAAAAASUVORK5CYII=\n",
"text/latex": [
"$$\\operatorname{atan}{\\left (\\frac{L_{2} \\sin{\\left (\\theta_{2} \\right )}}{L_{1} + L_{2} \\cos{\\left (\\theta_{2} \\right )}} \\right )}$$"
],
"text/plain": [
" ⎛ L₂⋅sin(θ₂) ⎞\n",
"atan⎜───────────────⎟\n",
" ⎝L₁ + L₂⋅cos(θ₂)⎠"
]
},
"execution_count": 22,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"beta2 = atan((L2 * sin(t2))/(L1 + L2 * cos(t2)))\n",
"beta2"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"By combining the above equation to the $\\beta$ formula, $\\theta_1$ can be calculated in the following manner."
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAWAAAAAlCAYAAAB8gzMnAAAABHNCSVQICAgIfAhkiAAADcVJREFU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\nRU5ErkJggg==\n",
"text/latex": [
"$$\\left [ - \\operatorname{atan}{\\left (\\frac{y}{x} \\right )} - \\operatorname{atan}{\\left (\\frac{L_{2} \\sin{\\left (\\theta_{2} \\right )}}{L_{1} + L_{2} \\cos{\\left (\\theta_{2} \\right )}} \\right )} + \\frac{\\pi}{2}\\right ]$$"
],
"text/plain": [
"⎡ ⎛y⎞ ⎛ L₂⋅sin(θ₂) ⎞ π⎤\n",
"⎢- atan⎜─⎟ - atan⎜───────────────⎟ + ─⎥\n",
"⎣ ⎝x⎠ ⎝L₁ + L₂⋅cos(θ₂)⎠ 2⎦"
]
},
"execution_count": 23,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"k2 = solve(beta2 - beta, t1)\n",
"k2"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now both $\\theta_1$ and $\\theta_2$ should be solved. To make sure, we draw the movement in the graph. Let's let the robot to draw the trajectory of the triangle."
]
},
{
"cell_type": "code",
"execution_count": 24,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
""
],
"text/plain": [
""
]
},
"execution_count": 24,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"subs = {\n",
" L1: 450.0 / 1000.0,\n",
" L2: 480.0 / 1000.0,\n",
" x2: 0.2,\n",
" y2: 0.2\n",
"}\n",
"\n",
"subs2 = {\n",
" L1: 450.0 / 1000.0,\n",
" L2: 480.0 / 1000.0,\n",
" t1: 0.0,\n",
" t2: 0.0\n",
"}\n",
"\n",
"def animate(i):\n",
" if i < 10:\n",
" subs[x2] = subs[x2] + 0.04\n",
" elif i < 20:\n",
" subs[y2] = subs[y2] + 0.04\n",
" else:\n",
" subs[x2] = subs[x2] - 0.04\n",
" subs[y2] = subs[y2] - 0.04\n",
" subs2[t2] = subs[t2] = k[0].evalf(subs=subs)\n",
" subs2[t1] = k2[0].evalf(subs=subs)\n",
" line.set_data([0, u.evalf(subs=subs2), x.evalf(subs=subs2)],\n",
" [0, v.evalf(subs=subs2), y.evalf(subs=subs2)])\n",
" infoline.set_text('x = %.2f, y = %.2f' % (subs[x2], subs[y2]))\n",
" return line, infoline\n",
"\n",
"animation.FuncAnimation(fig, animate, frames=30,\n",
" interval=25, blit=True, \n",
" init_func=init)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The trajectory of a triangle is drawn correctly by the robot hand. But if we imagine the real PA10 robot, elbow of the robot will hit the ground. In the above calculation $\\theta_2$ had two solutions. So, let's draw a graph using the second solution."
]
},
{
"cell_type": "code",
"execution_count": 25,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
""
],
"text/plain": [
""
]
},
"execution_count": 25,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"subs = {\n",
" L1: 450.0 / 1000.0,\n",
" L2: 480.0 / 1000.0,\n",
" x2: 0.2,\n",
" y2: 0.2\n",
"}\n",
"\n",
"subs2 = {\n",
" L1: 450.0 / 1000.0,\n",
" L2: 480.0 / 1000.0,\n",
" t1: 0.0,\n",
" t2: 0.0\n",
"}\n",
"\n",
"def animate(i):\n",
" if i < 10:\n",
" subs[x2] = subs[x2] + 0.04\n",
" elif i < 20:\n",
" subs[y2] = subs[y2] + 0.04\n",
" else:\n",
" subs[x2] = subs[x2] - 0.04\n",
" subs[y2] = subs[y2] - 0.04\n",
" subs2[t2] = subs[t2] = k[1].evalf(subs=subs)\n",
" subs2[t1] = k2[0].evalf(subs=subs)\n",
" line.set_data([0, u.evalf(subs=subs2), x.evalf(subs=subs2)],\n",
" [0, v.evalf(subs=subs2), y.evalf(subs=subs2)])\n",
" infoline.set_text('x = %.2f, y = %.2f' % (subs[x2], subs[y2]))\n",
" return line, infoline\n",
"\n",
"animation.FuncAnimation(fig, animate, frames=30,\n",
" interval=25, blit=True, \n",
" init_func=init)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now we get much preferred behavior. We can also understand the meaning of two solutions that came out. Even to realize the same hand coordinate, multiple solutions depending on the position of the elbow is possible due to the redundant degrees of freedom of the robot. Mathematically, both are correct. However in practice, there is a intended position of the elbow so we need to choose either solution."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"> FYI note:\n",
"> The \"position of the elbow\" of the robot can become a big problem. This time the ground is the problem, however, if we want to extend the robot arm in a narrow space, we have to take care both the hand and the elbow must not hit the obstacle. In this specific notebook we assumed the two-degree-of-freedom robot, however, it is also possible to design the robot with further redundant degrees of freedom. Such a robot has wide range of freedom the position of the elbow to realize the same hand coordinate. We can make an interesting robot since it is possible to proceed while avoiding obstacles to control the position of the elbow even in a narrow space, while on the other hands calculation of inverse kinematics becomes much challenging."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"> FYI note 2:\n",
"> However in practice, if we use a robot with high redundant degree of freedom, not only the calculation of inverse kinematics become difficult, but also, securing of hand coordinate accuracy become difficult because the backlash of each motor and distortion of each link will be accumulated. In practice, we try to make high degree of freedom of the robot at the same time from the simple kinematics. \"SCARA robot\" is one of such robot. In SCARA robot, xy-axis has the same structure as the two-degree-of-freedom robot we calculated in this notebook. There is additional z-axis direction linear joint close to the hand. Thanks to this structure, mechanical rigidity is easily secured, and it is an excellent design of the robot that can balance the high precision control and the ease of kinematic calculation."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Numerical method to solve inverse kinematics"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We have already studied how to calculate inverse kinematics using \"analytical method\" which takes the mathematical approach to calculate inverse function. But I think it was pretty tiring work even for the greatly simplified robot like we assumed here. If we want to calculate the joint angles of the complex robot with complex kinematics will be even more difficult task.\n",
"\n",
"In the analytical method, we tried to calculate joint angles using an inverse function. However, without using the inverse function, we can calculate the joint angles by iterative calculations using computer. Such method is called \"numerical method\". Here, we will explain the example of using Newton's method."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In Newton's method, we will approach to the solution by the following way.\n",
"\n",
"First of all, we give the appropriate initial value to the joint angles. \"Appropriate value\" is difficult than we think, but lets say we set a random value that was determined by shaking a dice.\n",
"\n",
"$$ \\theta_1 = initial value $$\n",
"\n",
"From the initial value of the joint angles, we calculate the hand coordinate by using the formula in forward kinematics.\n",
"\n",
"$$ r_1 = f(\\theta_1) $$\n",
"\n",
"Except for the case where the initial value that we selected randomly was actually the correct answer, this hand coordinate should be different from the target coordinate.\n",
"\n",
"We calculate the difference between the target coordinate and the current hand coordinate. This value will measure how the current coordinate and the target coordinate is distant in either direction in the xy-axis of the hand coordinate system.\n",
"\n",
"$$ r - r_1 $$\n",
"\n",
"Although, we obtained the measure in the hand coordinate system, we have to the decrease the measure by adjusting joint angles of the robot. To do that, in the Newton's method we use a derivative of the function. By substituting the value of the current joint angle to the differentiation of the forward kinematics function, we will get constant which is proportion to \"how much the xy-axis values in hand coordinate system will be changed by changing joint angle values\".\n",
"\n",
"$$ A_1 = f '(\\theta_1) $$\n",
"\n",
"Because the original forward kinematics formula contains trigonometric functions, this constant should make complex change depending on the joint angle. So the resulting constant only effective \"in the vicinity of the current joint angle\". But it is enough to make an approximation.\n",
"\n",
"We use the above constant as the \"glue\" to connect the joint angle to the hand xy-axis coordinate system.\n",
"\n",
"We place xy-axis hand coordinate system to the left side of the equation, the right side is the joint angle coordinate system and put them together with the glue. Here $r$ and $\\theta$ is the respective target value of the hand coordinate and the joint angles.\n",
"\n",
"$$ r - r_1 = A_1 (\\theta - \\theta_1) $$\n",
"\n",
"Strictly speaking, this does not means this equality is established. But we try to think at this specific moment, the approximately holds such relationship.\n",
"\n",
"Now we solve the above equation with respect to $\\theta$.\n",
"\n",
"$$ \\theta = \\theta_1 + A_1^{-1} (r - r_1) $$\n",
"\n",
"By replacing $r_1$ and $A_1$ to the actual values we calculated from the forward kinematics, we will get the following equation which determining the correct joint angle $\\theta$ from the target hand coordinate $r$ and the initial value $\\theta_1$.\n",
"\n",
"$$ \\theta = \\theta_1 + f'(\\theta_1)^{-1} (r - f(\\theta_1)) $$\n",
"\n",
"Since $f '(\\theta_1)$ is only an approximation near $\\theta_1$, we improve the accuracy by calculating repeatedly as follows.\n",
"\n",
"$$ \\theta_{i+1} = \\theta_i + k f'(\\theta_i)^{-1} (r - f(\\theta_i)) $$\n",
"\n",
"Here, $k$ is the coefficient used when $A=f '(\\theta)$ is not so creditable as an approximation (including our case). If this coefficient is set to small value, the approach to the solution will be little-by-little and required numbers of calculation will be increased, but can realize more reliable convergence."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Please note that in Newton's method, we can calculate the solution by using only the derivative of the function. Unlike the complex conversions required in analytical method to get inverse function, derivative of the function can be easily calculated."
]
},
{
"cell_type": "code",
"execution_count": 26,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAz4AAAAXCAYAAADKgJq7AAAABHNCSVQICAgIfAhkiAAACv5JREFU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\nURRFURRFURRFURRFUZSG+D//NU5yTtpc/wAAAABJRU5ErkJggg==\n",
"text/latex": [
"$$\\left [ \\left [ L_{1} \\cos{\\left (\\theta_{1} \\right )} + L_{2} \\cos{\\left (\\theta_{1} + \\theta_{2} \\right )}, \\quad - L_{1} \\sin{\\left (\\theta_{1} \\right )} - L_{2} \\sin{\\left (\\theta_{1} + \\theta_{2} \\right )}\\right ], \\quad \\left [ L_{2} \\cos{\\left (\\theta_{1} + \\theta_{2} \\right )}, \\quad - L_{2} \\sin{\\left (\\theta_{1} + \\theta_{2} \\right )}\\right ]\\right ]$$"
],
"text/plain": [
"[[L₁⋅cos(θ₁) + L₂⋅cos(θ₁ + θ₂), -L₁⋅sin(θ₁) - L₂⋅sin(θ₁ + θ₂)], [L₂⋅cos(θ₁ + θ\n",
"₂), -L₂⋅sin(θ₁ + θ₂)]]"
]
},
"execution_count": 26,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"xdt1 = diff(x, t1)\n",
"xdt2 = diff(x, t2)\n",
"ydt1 = diff(y, t1)\n",
"ydt2 = diff(y, t2)\n",
"j = [[xdt1, ydt1], [xdt2, ydt2]]\n",
"j"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Lets calculate $A$ and $A^{-1}$ from a suitable initial value."
]
},
{
"cell_type": "code",
"execution_count": 27,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/latex": [
"$$\\left[\\begin{matrix}0.918183831738908 & -0.140286316272702\\\\0.470431957363796 & -0.0953612787816294\\end{matrix}\\right]$$"
],
"text/plain": [
"⎡0.918183831738908 -0.140286316272702 ⎤\n",
"⎢ ⎥\n",
"⎣0.470431957363796 -0.0953612787816294⎦"
]
},
"execution_count": 27,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"subs = {\n",
" L1: 450.0 / 1000.0,\n",
" L2: 480.0 / 1000.0,\n",
" t1: 0.1,\n",
" t2: 0.1\n",
"}\n",
"\n",
"A = Matrix([[xdt1.evalf(subs=subs), ydt1.evalf(subs=subs)], [xdt2.evalf(subs=subs), ydt2.evalf(subs=subs)]])\n",
"A"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$A^{-1}$ is the inverse function of $A$, but it can be calculated easily because it is a constant. If $A$ is one-dimension\n",
"\n",
"$$A^{-1} = \\frac{1}{A}$$\n",
"\n",
"But, here in our case the input and output of the system is 2-dimension because the expression is $r = (x, y)$ and $\\theta = (\\theta_1, \\theta_2)$. Such case, $A$ will be a matrix. Inverse function (inverse matrix) of 2-dimension matrix can be calculated as follows.\n",
"\n",
"$$A^{-1} = \\begin{pmatrix} a & b \\\\ c & d \\end{pmatrix}^{-1} = \\frac{1}{ad-bc}\\begin{pmatrix} d & -b \\\\ -c & a \\end{pmatrix}$$\n",
"\n",
"This apparently looks like a complex formula, but we can calculate them instantly because it is simply a multiplication and division of each matrix element."
]
},
{
"cell_type": "code",
"execution_count": 28,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/latex": [
"$$\\left[\\begin{matrix}4.42224073456897 & -6.50557406790228\\\\21.8155984407579 & -42.5794409892144\\end{matrix}\\right]$$"
],
"text/plain": [
"⎡4.42224073456897 -6.50557406790228⎤\n",
"⎢ ⎥\n",
"⎣21.8155984407579 -42.5794409892144⎦"
]
},
"execution_count": 28,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"AI = A.pinv()\n",
"AI"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In this specific notebook we use function pinv() to calculate pseudo-inverse matrix instead of function inv() to calculate normal inverse matrix. To explain in brief, the stability of the calculation can be improved by using pseudo-inverse matrix in such a state in which the hand of the robot is fully extended.\n",
"\n",
"Let's write a program to find a solution by calculating literately."
]
},
{
"cell_type": "code",
"execution_count": 29,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"subs = {\n",
" L1: 450.0 / 1000.0,\n",
" L2: 480.0 / 1000.0,\n",
" t1: 0.0,\n",
" t2: 0.0\n",
"}\n",
"\n",
"def newton(t1_initial, t2_initial, target_x, target_y, times):\n",
" subs[t1] = t1_initial\n",
" subs[t2] = t2_initial\n",
" for i in range(0, times):\n",
" A = Matrix([[xdt1.evalf(subs=subs), ydt1.evalf(subs=subs)], [xdt2.evalf(subs=subs), ydt2.evalf(subs=subs)]])\n",
" AI = A.pinv()\n",
" t1_next = subs[t1] + 0.1 * (AI[0,0]*(target_x - x.evalf(subs=subs)) + AI[0,1]*(target_y - y.evalf(subs=subs)))\n",
" t2_next = subs[t2] + 0.1 * (AI[1,0]*(target_x - x.evalf(subs=subs)) + AI[1,1]*(target_y - y.evalf(subs=subs)))\n",
" subs[t1] = t1_next\n",
" subs[t2] = t2_next\n",
" return [subs[t1], subs[t2]]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's confirm the result by drawing a graph."
]
},
{
"cell_type": "code",
"execution_count": 30,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
""
],
"text/plain": [
""
]
},
"execution_count": 30,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"subs2 = {\n",
" L1: 450.0 / 1000.0,\n",
" L2: 480.0 / 1000.0,\n",
" t1: -0.5,\n",
" t2: 3.0\n",
"}\n",
"\n",
"target_x = 0.2\n",
"target_y = 0.2\n",
"\n",
"def animate(i):\n",
" global target_x, target_y\n",
" if i < 10:\n",
" target_x = target_x + 0.04\n",
" elif i < 20:\n",
" target_y = target_y + 0.04\n",
" else:\n",
" target_x = target_x - 0.04\n",
" target_y = target_y - 0.04\n",
" [subs2[t1], subs2[t2]] = newton(subs2[t1], subs2[t2], target_x, target_y, 10)\n",
" line.set_data([0, u.evalf(subs=subs2), x.evalf(subs=subs2)],\n",
" [0, v.evalf(subs=subs2), y.evalf(subs=subs2)])\n",
" infoline.set_text('x = %.2f, y = %.2f' % (target_x, target_y))\n",
" return line, infoline\n",
"\n",
"animation.FuncAnimation(fig, animate, frames=30,\n",
" interval=25, blit=True, \n",
" init_func=init)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"It seems it is correctly estimating the solution.\n",
"\n",
"We have calculated the inverse kinematics by using a numerical method without having to derive the inverse function.\n",
"\n",
"Newton's method is a very powerful algorithm that can approximate optimal parameters even for the very complex function which is difficult to derive an inverse function. Not only limited to robotic application, it is applied to wide variety of optimization problems.\n",
"On the other hand, we have to note that the convergence is not guaranteed in such method, but actually depend on how appropriate we can select the initial value. You need to take advantage of the method at the same time understanding their characters.\n",
"\n",
"Although there are few characters to be cared when using the numerical method, we can easily calculate the inverse kinematics even for the robot with complex structure it only from some repetitive calculations. In recent years, performance the computer has significantly increased. Even the small embedded processor is capable to calculate the inverse kinematics at practical speed. From this background, numerical method has been widely used in many advanced robotic systems."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Implementation to the robot"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You might have expected the content of this notebook is programming with Python, but probably have noticed it was actually a continuing transformation of the equations. But it is now over!\n",
"\n",
"Finally we will program the above inverse kinematics in C ++ and control the robot.\n",
"\n",
"In the [previous article (in Japanese)](http://tech.mid-japan.com/blog/2014/07/10/learn-pid-control-and-rtm/) we have already implemented a PID controller to PA10 robot and did the adjustment. In addition, we know that by using the middle-ware framework such as RTM, we can developed the system divided into parts which is called \"component\". This time we will try to compose a system of the following design by rearranging the PID controller was developed previously."
]
},
{
"cell_type": "code",
"execution_count": 31,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n",
"\n",
"\n",
"\n"
],
"text/plain": [
""
]
},
"execution_count": 31,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"graphviz.Source('''\n",
"digraph G {\n",
" rankdir=LR;\n",
" input -> IK [label=\"hand position\"]\n",
" IK -> PID_control [label=\"joint angles\"]\n",
" PID_control -> PA10 [label=\"joint torques\"]\n",
" PA10 -> PID_control [label=\"joint angles\"]\n",
"}\n",
"''')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The main part of today is the \"inverse kinematics component\" located at the center of the figure.\n",
"\n",
"We have already calculated the inverse kinematics in both the analytical methods and numerical methods. Here, to thank to our valuable work on converting equations, let's implement our component using the analytical method.\n",
"\n",
"We get the following formula as the final outcome obtained from the analytical method:"
]
},
{
"cell_type": "code",
"execution_count": 32,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAqAAAAAlCAYAAACd3luHAAAABHNCSVQICAgIfAhkiAAAFcJJREFU\neJztnXm0FNWdxz/vASKCgCPiDvgABcUFRVFBkBGMYlwnCIpiIqgnJmMEgxoxsSMGFRKJyRgPIYmd\nMCgzDpq4EMdsLxOdZBI1Y4yaccUlzsQxUScacWX++FalqutVVd/aX/Pu55w+71V31a1fd9Wt+7u/\n7YLFYrFYLBaLxVIibRHvDw5svw5sLlgWi6Us5gIjgSurFqSH0O78/aBSKSwWi8VSNtsAW/m2/wK8\nE3fAJuA+32tAYaJZLOVyIvBPRE++0jIEuAJ4F7gLWALsmKG9h4CPJjymX2C7P/BZp51lMccNA05N\neK4kdAC3ogeRxWKxWHoOS/F0yecxGNc2FiuPxVIJewCPAoMKan848hTslkNbxwO7Jth/N+Ajvu02\n4G7gIGf7FmDPmONnAZOTCJiQw4HvA70LPIelNZkMXFO1EBaLJTEjSDae1rAKqKWH0gmcXGD7C4Df\nFdh+HEsD2/OBe3zb3wNmxhzfDtRzlinIZcAXCz6HpbXYF7iD1p+YtAMnVC2ExZIjg4A1xHuudkQe\nxWDYZhQ1rAJq6YGcDDxY8DnWAf+QYP8dgUXI+jgHTwGcDvwSuMTZPh74LV7nPQu4HbnYAY4BTvK1\n2wY85bTr8jwwqYk8q4CdEsiflN7Aw8ARBZ7D0jpsg+7z4Tm2uS2wHoWVlEUbsBo4pMRzpmUisBA9\nS34ATKlUGnNaTe5WkbeZnNOR8SIuZG0M8ra1x+zjUsMqoLnQJ8e2Tslw7OeB8XkJsgXzMDC7wPbb\ngJcJt7C2A38PXOy8XD6NLJUu5/r+X4o6q8tVqJO73IF336yicQY6AcWi1oBLgeXAW8DACDlcFgPH\nhryfJ+5EIO8Y3DLJ0l8tHlciq3heLEDPw83INVgWl6F+1t0ZAFzt2z4VJYQkCfWpglaTu1XkNZXz\nK8hQEsdi4HKDc9awCmhmZgFn59TWXsB3Mxy/NZq5jMxHnC2SSaiSw9YFnmM88B5dXRFjkWtud2d7\nPV5c5ljgRWQFuo7G2NEajQpojUY3ex2vI28InPN84H7f9iLgphg5XM5BFQKKpA34L2TVbUWy9leL\nGAq8gCZFeVOmAroX8CT5GiSKYj9UicIdKwai36rIBMQ8aDW5W0VeUzlHAK8CO8S01QeFnzXzZtTw\nKaAmJlNLI9OQC/FbObU3FyWIpGUTUji+A/TKRaItj3nIYripwHNMR5a91wLvn4yywE9ztp/GUwLf\nRYlRi53/O2ksVxHk/Yj3g4r1QJRFD1L45gNfjpHDZVv0oCmSzSiu6BMFn6cowvrrIcBtKMnqEeCb\ndD9rR3djAXLt/V/VgmSkBtyI+m+ZtKNn2uOoT70H/AIZIw6LOOYR57NnnG23/z9ZnJihJJW9u8ht\nSqvIayrnRuBHwHkxbb0LfBVVgsnMxjwaKZCRwEvkGztkwiDgV+RbTuYhupbPScM64Mwc2imSqq7b\nCyQvaZSUfwW+EHhvIbI69kXKHUhJ2cX5/wrgAN/+6/EsQjW6WkD923W87/SDwHln4blWzsardxol\nh8tXKOfajEHKdNyMuhlVWZyC/fVA4F48y/cA4N9QOMaIUiVrLZ5GlRGKoCwL6FDgbWDnEs4VxZHo\n+6apIrAG+FKu0iTjSNLJnkXuKp4brSJvnJzzkOcqjsHIALN9zD41tgAX/IXImlWkSzWMa51z58Uh\n6KLnwaHAYzm1VRRVXLe90EOuqBCFiei+2AT8M4qDuRopIS/RmN07icZYsYuREnoGcn+78aAzgQeQ\na36yb/sB5/+5aJb6QzSI30DjINgb+LrT5qfoGm8ZlMMlr3vRhN+TbVKwmvKtjGH99W5gVOC98eie\nW1eGUC3IODRQFeWBK0sBnU91VS9cPou+71EJjzsbxYZXGYudRvascpf93GgVeZvJORpdq9FN2rkL\n+HjM5zW2AAV0PfDTks/ZH/gT8dp9Uq4nv6SPXsidtW9O7RVBFddtLvAG1Se9DMQsSDsNU4GPZZTj\naMqNy7yVbGWf6pRvYQzrr2+gCgNDA++/CrxShlAtyCK6xi3nSVkK6FqyhU/lwQ/RqjJJvHIz8XIY\ntqY6S31S2fOQu57yuDS0irymcv4vqsASxyKkhEZRYwuIAZ2MLExlchxSzP+YU3u9UDxp0H2alveB\nf0cleborVVy3/ZCVouqlZOcgS2kfFC+aJz9Fll6TGOAwOYaih86dOcsVx2N0TYIqi62QqzzsFTUY\nRvXXZ1E5rf6B998mn9CaLZGDgd9ULUQOTAD+s8Lz90ExfA+i7GUTpiBvyQZUcu0YqgkhSCp7d5Hb\nlFaRN4mcLwD7NGnv52icNzL4dKfiv3+LkmmeAbZDg+p5eDUDZyMrz3ZowJyGCm3fiVyQpu2AlmOc\ngZSTs4C/QavItKEfbxldZ+gzkIIXRgfS/EcANzsvl4vQA3dOyPf9GQrADnIQcsu6M/kFzncYjMzx\nV6AYqiCP0hhTWAZlXbc01wz0+/0+65fMyGxUgH0ZUmSKqIN5DXA68W70KDneQC77MnkSxYK2U94a\n8dsD30YP2Thl/Ui6Wuqj+uuhKK72f3zv7YKU0s70ouaCybMQ5DW5CHlQ3kLK8zIak/bGOPu8iZSH\nISik5r8DbQ1DrtW3kGKxCVjhHOc/372Zvlk4c/FW87oGLf2XpC5vEtrRs+UPBbVvwgQ0YTKd1Hcg\n69S2gfeLWhkujiSydwe5k4zJVctrqo8klfNVmtfXfcw5voNwHcWIjWkPTMl85K4a4Wzvih5Yvw7Z\n9zyirQsm7WyFMoJBCUX3oYviauyXIpdakAeIzgL7mtPup+g6s38YrRQQ5CbCC4OPRtlkrjx14AkU\n6zcJDdYXRchxOcomLIuyrlvaa4az7+pmX8RSOtPRwzytVaBOMtdUb5QothD1oxtQRYBDUTb2FOf/\nAwifvUf11zCWIY9EUUk2Jpj2zVNRrPL+zvZU4M9owudyIlI0J/jeW4ieiX5Fvh96Vo1xtkc6MvgX\nSQApuq1ahstle3T/FrmyWjMucWQ4rkIZ0lKV7HWSu7SzjMlZqZNc3jT6iAnrUfJqM94g2hNbo5vF\ngO6HUvjPCLz/IqqNGGQdUirStjMDz3L2R/Sj+vkMjbN1l1cIL2Q+GVmeQBfnX3yfDUYD0fmBY/oi\n103YQPc1Gt15twL/4fy/O7JgDQk5Duc8j0d8ljdlXre01wzUAcPksVTLAWgASmuxr5PswXw68CHf\n9m14IUh3d929gbj+GqQDKXBXJZAtb0z75ng0KZzne+8E5BZ1a9Lui6yZwbrHB6Hr5y+bcwJKLnLD\nGLZHk2L/82yAc1x3XRnGlN3R98g7nCYJG9D4UoUFMytVyV4nnUKXdkzOSp1k8qbRR0xZg+J2m/Ei\nSoANo4ZPAU3rgt8f/TCmiR2/JjpJYhnSmP1a+XA0Y+8M2f9IVGcvbTu/RQ/J/ZAbd2Wgnf3RLCHI\nILrWeAQtefgr5zwzaFz+8Ag0yAXdeR9GN0dYXOK1NCpTh+Mla7yAVs+J4gPiXYutet3SXjOQVebt\niM8s1eG6d5slIHwbzzrnZxjKSn8n5LP5dF121e+G2gn1lQ9Qv26WWBjXX/30dc6zmuISzkww7ZvL\nkQt5re+9O5yXywo0+f5O4BxuubBhKO4L1EcHofqCG1BViKAi7g7krxt9k+6LG4rR12DfPJ+7Lr2Q\n9e03RP+W+6JrkZYi5IZyZM/ruQHZxmRT8pI3jT5iSj+iDT1+XkchP6nZmOXgBAxGHTloEj4LaerB\nVWX2QYPA0RnbAcUvvUlj4e8+6CH6mZD93yE+Y30h+uH9D6TlKHMs2IHXY5atPpZks+wl6OYrmqqu\nW9JrBoo1vDriM0t1dKB7YlrK4+ukzw5dgpYkBT3co2K7XUz76xq8mqtVYdqndkAK+A1EMyRmH9eF\nGlz//BJkAdnsvD4Z+HwYxZZFK4tt0PcIWpnLwrVAXx/xeT+Ki3/NSpWy18mWVZ50TM5KnXTyJtFH\nTLkXs3C2+4kuSF+jG2XBj0KzoZ8H3p+KXF6voZViXKahh6s7YAxGJvCk7bht3U/jzOJYNLO/xWnP\nX5T7NWR9i+JDwE9otLZNQUHWfsvJIOTeMpndHeXI5x8gO2L23x5l5RZNVdct6TUDuQ/jrBSb7avQ\nVxTuNSnbOt0LuYdcV9LOKGEoCtP+WkNxYZ/zvTcvfNdCMe1THWggeiCmrZEx+5yJkgyCE95r0e81\nDtWxDbr73OsdrBoA1d+rSe7lvyDLcJhxowzcEIYoa9Z8lOzpsi2aSDVLIimDJLJPRMpUDVWgqDp0\nI+mYXBWm+kgShmGWWNQHw8TSqhVQ1/zuTyDpiy6ye3P6g3uPQA/RN5ztC9GsPmk77ehi/CQgzzzg\nx8gCPB2tcuLyLPEK6HAaVwro7xwf7GR/B9we0UY/NEtxrS0zkJvCLVPRTry5f0/CE4Dyporrluaa\nubIOiPkubfZV6CsKdzEC0/IxefERpKC5sdJ9Ud+N6ttx/dXlDHR/Lg28PzmwvRfFL8Jg2qdedv7/\nc0gbe6DkkKh9jgb2Bi7AG8xuozHh4VFkEX6m8dC/PgPCYv+qvleT3suPUp1CN9X5+7OQz3ZEkyxX\niVuAxo1TqH7MB3PZB6AEtpVIAV3tvF9mIfmsY3JVmOojpvR22jTxsA5Ghp+mVH0zPoUUE3cG0QuZ\n3gcgTXsIKv6O7/ONzv8Ho5vgpRTtHIh+pM6APHui8kC9UcKRv6DqfeihG8VzNA5iK9BMIHjB5xBd\nvHgmupn3QZmkHTTOYJYQXWanHQ14JkHCWaniuqW5ZqASTMEi4Zbqca9J2WVsPk1j0tFz6L6bGbF/\nXH8FTYpWonv4H32vdTSWN5mG6tGuDTaQM6Z96lnkUgtalI5AVtzOiH06kCJwAY1lzybQ+OwZipIh\ngiEJbyKFthUTZ4LcT/S660XSG12n3yGXqp8j0FjlLxn2DXJaozsHksjegUI63HCNe5BCaFqNIg+y\njMlVYqqPmOKu7hb0rIQxCG/ymoqNWQ5OyCikNKxEpQ7GogfXj4Bv0ege2x+Zvr8ELKZRgU7Szglo\nFhNMwpqNHqqr6KpszkAz3ijG4NWd+yLqRH8KyLgT8bUBhziyrnDa6I+Ck1c532lGzLEHo8GnLMq+\nbmmuGage4C8Nvs8Q9JB+15FnCfGu2Typ8txB/GWy+qOajh9FiS1xDEMlfUw5C7my0k6C68CIFMet\npnE5uV5ogA4rsNysv4KqMkS5bP0W0b3RgLsxhcxJMe1Tg1F5qW+iZ871yP3pvyb+fb6Kkqwmhpxz\nKlJcr0bPwJvoGh/q8gjRK6o0c7lm7St59rXDkaWnrPW6xyAl/0l0f72OSovdgzxDz+Lde2HXaDPp\n+kwepJG9zfnftT67uQTjM8hRJ9lvkGVMzoM66a6ZiT6ShMto7gly2UR0bH+NblaGqVXoiwYbE/N/\nOypMfVPg/QtJXwKhGWuATxTUditzOqo7aMJw9IDbrdmOhsxqvkth507DbshFDXro3423WtEtyNoc\nxyy6up2j+ALZVsOpU/xgWkR/reXcXityM+ETmgE0JgyeirwlwWdu1r6SV19rQ27OVqnDWaUCmgdr\nkBEjC3Va6zeok13eKH0kCY9jpmzviu6zqEldDauApuZKFA8S5BYaB9OT0SxgdGC/+ymmXtgezvnL\nmom3Enti/uBdgFxDeVFLsG/e506D32IXTGL4HtFuapd2zNd3/26CfcM4kOLv9yL6q63IIMU+bNnX\n/VDygutyHYj6btCynrWv5NnXziG/5ZSLppUV0LPR2BsXV25CGc+NPEkjr6k+YspxmLvuj0ZVMKKo\nYRXQ1AxANSmDCQuvIHM8aBm+Z+i69OYomhe8TkMfpCiUvQRnK/EcytptxjryLf9RS7BvmnMfhu67\nc1C9RbfQ+hTn3Gcid+ORzvs7oiXaZqH7s+5r6xi8FWvaUDiH34L7PGaxV6uQ6zqONhT7WUWWuClF\n9NdpeBbmnszehLsDTV2uzfpKO11rBZsc347Kc13svExoRyE+WdzCZdGqCuhMvIUQtqY1v0OZmOgj\npvRBcZ9jmu3osBSFKERRwyqgmTiMxlVUQIPKchRrsRYt6RfkcxRTM+5KtOazJZobae5+aEOB01FL\n6zUb1MKoGe4Xd+6oQXFntDziLs72xSheczSNy7G2ocoIo1Aw/XzfZ+f6/l+FV1JmAoqRq6FlTpej\nWLcBMfK4LCa+Xi4oo/R9ils9JA/y7q+9sdZPP08QHqfoJ8zl2qyfDkKTrIciPo87/gRUHg5Usuig\nkH3C6EAxds0WVaiaVlRAp6Bn1k7O6ySqSfxqJUz0EVO+jPqFKb8gfpJdwyqgmTkWBcon4U7iywGl\nJe1qVj2JicCrxNcDHY8yL4N1/cbSfFADWRevRklP7uu+wPaSFOeOGhQ/ibccnJ8rUdC8n3XA5532\nXkQWm+tojIHzZzSfj9zPLovwFPhmg/Q5wNwQuYIybmiyT9UU1V8t4hKiC5FDtMs1rq/46YxoN+74\nC/EmVcvpuoZ9HAehuObuyFw0Cd+MngXBxQG6Kx0ofj+Y3Dcw7iBLbgxC5dtMGYk8W1vF7FPDKqCW\nHsiDxCcFLSZcobvM939nwnPWDPeLO3fUoHgB4TXZVtK1bNB6pByPQi6Vqahg+FN4D4sf+/a/FM+F\n04aqP7jLxDUbpBcRHyvajsoBfThmH8uWzxA0zmwb8lmcy9Wkn0J0X407vq9Pnu/jeRcsFktzVtC8\n3FeNbrQSksVSFlcgq0sU0+laQ3Uhivktmrhz3+i8QK5rt6TU7Whg3sN3zBzkthyH17fbnePWIkvI\nPiig/BLgYbzC6O/72nkar2D4x5AC+7CzHSWPywjiy5XNRvF/RcRDW1qHV1Bpp2DljikovGQDcrke\n42y7ZO2ncce/jWqUTkJ95CXDNi2Wns4OwInI7W+Mdd9aegp3IevdSSgD22UiWiFkKqpNdzmq9TYJ\nWQyLXOnE5NzvoYExOCi+4HyXGqqxuhnV03sSrXRzOSq9sTvwcTTAzkQPiXGo3ucGvBJVT+DFld6O\nSm6cg+La/LPatyPkcdkOJX2FsRWKUz2X9MvBWbYcVqDapDejJLcO1E+DVtFBZO+npscPRMliV6X/\nWhZLj2Ml8oy9mUdjG/NoxGLpZuyOylOkjSHqTLj/aSnPE2QgGjCLZCqydpoQJc/RwPExxy1Frn+L\nxWUsSurM2xjSmfK4c1GYSh9kLbVYLPGchiaTJtSwMaCWHsyxyB2dpp5cZ76iGFPWoHgNWhkojTxD\nacyqD3IUiquzXhdLkEPJL4GnH/J0/AHFI/eL372B2cgj8ApKWhyXk0wWy5bKRJQpbzqe1jBQQN9B\nLjv3FRYobrG0KqcQnZEeRpZBLStlDoqDaV4vNUqeuBI0o1EcaZm/m8VisViq5zo8XfJlDBRQi8Vi\nsVgsFovFYrFYLBaLxWKxWCwWi8VisVgsFovFYrFYLBZL9+X/AfoS/isNbzMWAAAAAElFTkSuQmCC\n",
"text/latex": [
"$$\\left [ - \\operatorname{atan}{\\left (\\frac{y}{x} \\right )} - \\operatorname{atan}{\\left (\\frac{L_{2} \\sin{\\left (\\theta_{2} \\right )}}{L_{1} + L_{2} \\cos{\\left (\\theta_{2} \\right )}} \\right )} + \\frac{\\pi}{2}, \\quad \\operatorname{acos}{\\left (\\frac{1}{2 L_{1} L_{2}} \\left(- L_{1}^{2} - L_{2}^{2} + x^{2} + y^{2}\\right) \\right )}\\right ]$$"
],
"text/plain": [
"⎡ ⎛ 2 2 2 2⎞⎤\n",
"⎢ ⎛y⎞ ⎛ L₂⋅sin(θ₂) ⎞ π ⎜- L₁ - L₂ + x + y ⎟⎥\n",
"⎢- atan⎜─⎟ - atan⎜───────────────⎟ + ─, acos⎜─────────────────────⎟⎥\n",
"⎣ ⎝x⎠ ⎝L₁ + L₂⋅cos(θ₂)⎠ 2 ⎝ 2⋅L₁⋅L₂ ⎠⎦"
]
},
"execution_count": 32,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"[k2[0], k[1]]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Implementation of this formula in C++ is as follows.\n",
"\n",
"```cpp\n",
"#include \n",
"\n",
"#define L1 0.45\n",
"#define L2 0.48\n",
"\n",
"double calc_t1(double x, double y, double t2) {\n",
" double t1;\n",
" t1 = - atan2(y, x) - atan2(L2*sin(t2), L1+L2*cos(t2)) + M_PI/2.0;\n",
" return t1;\n",
"}\n",
"\n",
"double calc_t2(double x, double y) {\n",
" double t2;\n",
" t2 = acos((-L1*L1 - L2*L2 + x*x + y*y)/(2*L1*L2));\n",
" return t2;\n",
"}\n",
"```"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The entire source code is available at the following repository.\n",
"\n",
"https://github.com/devrt/inverse-kinematics-primer\n",
"\n",
"Please first read file InverseKinematics.cpp, HandMotionGeneration.cpp, PIDController.cpp which is the source code of each component.\n",
"\n",
"To start the simulation, it require hrpsys-base installed on Linux environment.\n",
"It is possible to launch the environment using the Vagrant environment, we have introduced [several times before] (tech.mid-japan.com/blog/2014/06/28/vagrant-hrpsys-watchdog/). It is recommended particular for those who are using Windows.\n",
"\n",
"Once startup the environment and logged in, please checkout the repository and then start the script.\n",
"\n",
"```\n",
"git clone https://github.com/devrt/inverse-kinematics-primer.git\n",
"cd inverese-kinematic-primer\n",
"./run.sh\n",
"```\n",
"\n",
"Program to draw a triangular trajectory by the hand has been implemented as a default."
]
},
{
"cell_type": "code",
"execution_count": 33,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
""
],
"text/plain": [
""
]
},
"execution_count": 33,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"Image(url=\"http://devrt.tk/build/repo/53d8a5497466b5fd34fe25bb.gif\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"It seems it has worked without problems.\n",
"\n",
"Let's reach to the box by editing the HandMotionGeneration.cpp."
]
},
{
"cell_type": "code",
"execution_count": 34,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
""
],
"text/plain": [
""
]
},
"execution_count": 34,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"Image(url=\"http://devrt.tk/build/repo/53d8aa637466b5fd34fe25bd.gif\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We have successfully touched the box ! (It is also possible to pick a box, by using fingers. Please go ahead and try to modify the source code.)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Appendix"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In this specific notebook we have calculated and implemented the inverse kinematics formula from scratch. However, various libraries to solve the inverse kinematics already exist, because they are frequently used to control the robot.\n",
"\n",
"Currently, the most popular library used in the robotics society is IKFast developed by Rosen Diankov.\n",
"\n",
"http://openrave.org/docs/latest_stable/openravepy/ikfast/\n",
"\n",
"IKFast can automatically the analytical solution given the structure of the robot (it internally uses sympy). It will also convert and output the solved equation to C++ code.\n",
"\n",
"Because it is using analytical solution, the results of inverse kinematics is stable and work very fast.\n",
"\n",
"To take advantage of the IKFast from ROS, you can use the following package.\n",
"\n",
"http://moveit.ros.org/wiki/Kinematics/IKFast\n",
"\n",
"Package utilize OpenRAVE (parent project of IKFast) from the RTM have been developed and published by Okada's lab (Tamagawa University).\n",
"\n",
"https://code.google.com/p/rtc-openrave/\n",
"\n",
"\n",
"There is also a library to calculate the inverse kinematics in OpenHRP.\n",
"\n",
"We have to be careful about the stability, because the inverse kinematics is calculated by numerical method, but it is very convenient because it can calculate the inverse kinematics from the same VRML model as used in the simulation.\n",
"\n",
"http://www.openrtp.jp/openhrp3/jp/reference/idl/html/interfaceOpenHRP_1_1DynamicsSimulator.html\n",
"\n",
"By using this library, you can create a demo application, such as the following video (see the source code used in this demo from the following link).\n",
"\n",
"https://github.com/yosuke/OpenHRIWorlds/blob/master/openhriworlds/BlocksWorld.py"
]
},
{
"cell_type": "code",
"execution_count": 35,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/jpeg": "/9j/4AAQSkZJRgABAQAAAQABAAD/2wCEABALDA4MChAODQ4SERATGCgaGBYWGDEjJR0oOjM9PDkz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\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAP/Z\n",
"text/html": [
"\n",
" \n",
" "
],
"text/plain": [
""
]
},
"execution_count": 35,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"from IPython.display import YouTubeVideo\n",
"YouTubeVideo('HQBCnuJBs54')"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": false
},
"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.5.2"
}
},
"nbformat": 4,
"nbformat_minor": 0
}
![](\"http://devrt.tk/build/repo/53d8a5497466b5fd34fe25bb.gif\"/)
"
],
"text/plain": [
"![](\"http://devrt.tk/build/repo/53d8aa637466b5fd34fe25bd.gif\"/)
"
],
"text/plain": [
"