{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# このノートについて" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "ロボットアームの順運動学と逆運動学を手を動かしながら学習しましょう。三角関数を使った順運動学の立式からニュートン法を使った関節角推定までを解説します。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Written by Yosuke Matsusaka (MID Academic Promotions, Inc. [@yosuke](https://twitter.com/yosuke))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "このノートはCreative CommonsのBY-SAライセンスで公開します。![CC BY-SA](http://i.creativecommons.org/l/by-sa/3.0/88x31.png)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# 準備" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "このノートはipython notebookを使った手を動かしながら学べる教材になっています。Ubuntu上では\n", "```\n", "sudo apt-get install imagemagick asymptote graphviz\n", "sudo pip3 install jupyter matplotlib sympy graphviz\n", "jupyter notebook\n", "```\n", "と入力することで環境を立ち上げることができます。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "ipython notebookを立ち上げて新規ノートを作成したら、まずグラフ描画と数式処理のライブラリを読み込みます。" ] }, { "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": [ "以下も入力してください。この部分はアニメーション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": [ "グラフ領域を初期化します。このグラフ上でロボットアームの動作を確かめていきます。" ] }, { "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": [ "以下は説明のための作図ライブラリです（インストールは任意です）。" ] }, { "cell_type": "code", "execution_count": 4, "metadata": { "collapsed": false }, "outputs": [], "source": [ "import graphviz" ] }, { "cell_type": "code", "execution_count": 5, "metadata": { "collapsed": false }, "outputs": [], "source": [ "import subprocess # to run asymptote\n", "import shlex" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "では、はじめましょう。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# ロボットアームの順運動学" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "「順運動学」とはロボットアームの各関節角が与えられたときに手先の位置を求める作業のことです。" ] }, { "cell_type": "code", "execution_count": 6, "metadata": { "collapsed": false }, "outputs": [ { "data": { "image/svg+xml": [ "\n", "\n", "\n", "\n", "\n", "\n", "G\n", "\n", "\n", "関節角\n", "\n", "関節角\n", "\n", "\n", "手先の位置\n", "\n", "手先の位置\n", "\n", "\n", "関節角->手先の位置\n", "\n", "\n", "求める\n", "\n", "\n", "\n" ], "text/plain": [ "" ] }, "execution_count": 6, "metadata": {}, "output_type": "execute_result" } ], "source": [ "graphviz.Source('digraph G { rankdir=LR; 関節角 -> 手先の位置 [label=\"求める\"] }')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "「学」の字が入っているので学問を連想するのですが、そのような大げさなものではありません。実際は以下のような作業です。\n", "\n", "順運動学を求めるにはロボットアームの構造を知る必要があります。今回シミュレートするロボットアームは三菱重工製のPA10-7Cです。技術情報が以下のページで公開されています。\n", "\n", "https://www.mhi.co.jp/products/detail/pa10_6c_7c.html\n", "\n", "ロボットアームの各関節の配置と関節間の距離を以下から知ることができます。\n", "\n", "https://www.mhi.co.jp/products/detail/images/7c_mass.jpg" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "PA10は本来7自由度（7関節）あるロボットアームですが、ここでは大幅に簡略化して上下方向の2自由度（2関節）のみを考慮します。\n", "\n", "三菱重工の技術情報から、PA10は下図のような根元からたどって第1関節と第2関節の間が450mm、第2関節と手先の間が480mmある構造をしています。この情報を使って第1関節と第2関節の角度が与えられた時の手先の位置を求めてみましょう。" ] }, { "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": [ "![PA10 simplified](pa10-simplified.png)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "はじめに各関節の角度を変数として定義します。" ] }, { "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": [ "各関節間の距離を定数として定義します。" ] }, { "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": [ "手先の位置を求める前に、まずは第2関節の位置を求めます。第2関節の位置は三角関数を使って以下のように求まるでしょう。" ] }, { "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": [ "次に手先の位置を求めます。第2関節の位置に手先までの距離を加算します。" ] }, { "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": [ "これで手先の位置が求まったはずですが、念の為、グラフで描いて確認します。" ] }, { "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": [ "問題無いようです。順運動学が無事に求まりました。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# ロボットアームの逆運動学" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "逆運動学とは順運動学の逆に「手先の位置から各関節の角度を求める」作業です。" ] }, { "cell_type": "code", "execution_count": 13, "metadata": { "collapsed": false }, "outputs": [ { "data": { "image/svg+xml": [ "\n", "\n", "\n", "\n", "\n", "\n", "G\n", "\n", "\n", "手先の位置\n", "\n", "手先の位置\n", "\n", "\n", "関節角\n", "\n", "関節角\n", "\n", "\n", "手先の位置->関節角\n", "\n", "\n", "求める\n", "\n", "\n", "\n" ], "text/plain": [ "" ] }, "execution_count": 13, "metadata": {}, "output_type": "execute_result" } ], "source": [ "graphviz.Source('digraph G { rankdir=LR; 手先の位置 -> 関節角 [label=\"求める\"] }')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "ロボットアームを自由に操ってピックアンドプレースのような動作をさせるには逆運動学を求める必要があります。順運動学より多少難しくなりますが、「学」の字から想像されるほど難しいものではありません。\n", "\n", "逆運動学を求めるには、解析的に求める方法と数値的に求める方法の2つがあります。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 解析的に求める方法" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "順運動学はすでに求まっているので、その逆を求める、数学的に言うと逆関数を求める、ということになります。数学的なセンスでこのような逆関数を求めるのが解析的な方法です。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "順運動学は最終的に以下の2式になりました。" ] }, { "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": [ "解析的な方法では、式をパズルのように組み合わせ、試行錯誤しながら変換していきます。\n", "\n", "三角関数の変換では式を2乗して足し合わせると整理できることがあることが知られています。" ] }, { "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": [ "この式はまだ複雑に見えますが、以下に示す三角関数の各定理を用いると大幅に単純化することができます。\n", "\n", "ピタゴラスの定理: $ cos^2 \\theta + sin^2 \\theta = 1 $\n", "\n", "加法定理: $ cos(\\alpha - \\beta) = cos(\\alpha) cos(\\beta) + sin(\\alpha) sin(\\beta) $\n", "\n", "これを踏まえつつ、ここではsympyに自動で整理してもらいます（手抜きです）。" ] }, { "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": [ "整理すると$\\theta_1$が消えてしまいます（いつもこのようなことが起きるわけでなくラッキーなパターンです）。\n", "\n", "これだけ単純な式まで変換できると逆関数の計算は簡単です。$\\theta_2$は$cos$の逆関数$acos$を使って以下のように解けます。解が2つ出てきますが、これについては後で解説します。" ] }, { "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": [ "> 余談：\n", "> sympyのsolveは高機能な求解アルゴリズムを提供してくれます。これを使えば逆運動学を直接解けるのでは？とも思ってしまうのですが、元の整理する前の順運動学の式を与えても自動で解くことはできません。sympyが低機能なわけでなく逆運動学に必要な変換がかなり複雑なのです。このノート末尾の「おまけ」の中で言及しているIKFastのような、式を解きやすい形で立式して自動で求解してくれる強力なライブラリも近年開発されています。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "$\\theta_2$が求まったので次に$\\theta_1$を求めましょう。今回は、余弦定理を使って解いてみます。\n", "\n", "まずは第1関節から手先まで補助線を引き、補助線と$x$軸の成す角を$\\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": [ "![PA10 fist joint](pa10-firstjoint.png)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "$\\alpha$は$atan$を使って以下のように求まります。" ] }, { "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": [ "ここでさらに" ] }, { "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": [ "なる角$\\beta$を仮定すると、第2関節を中心とした以下の様な三角形に書き下すことができます。" ] }, { "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": [ "![PA10 second joint](pa10-secondjoint.png)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "余弦定理（を導出する前の三角形の分解と同様の考え方）から$\\beta$と$\\theta_2$の間には以下の関係が成り立つはずです。" ] }, { "cell_type": "code", "execution_count": 22, "metadata": { "collapsed": false }, "outputs": [ { "data": { "image/png": "iVBORw0KGgoAAAANSUhEUgAAAJYAAAAlCAYAAACgXxA5AAAABHNCSVQICAgIfAhkiAAACAtJREFU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\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": [ "上記の式を$\\beta$の式と組み合わせると$\\theta_1$は以下のようにして計算できます。" ] }, { "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": [ "これで$\\theta_1$と$\\theta_2$の両者が求まったはずですが、グラフで動きを確認します。試しに三角形の軌跡を描かせてみましょう。" ] }, { "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": [ "指定した三角形の軌跡を手先で正しく描いてくれてはいるのですが、PA10の実物を想像すると、このままでは肘が地面にぶつかってしまいます。先ほどの計算で$\\theta_2$には2つの解があったと思うのですが、2番目の解を使ってグラフを描いてみましょう。" ] }, { "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": [ "これで意図した動作になったのではないかと思います。先ほど2つの解が出てきた意味もこれでわかったと思うのですが、冗長な自由度があるロボットは同じ手先座標にしようと思っても肘の位置により複数の解が求まり、数学的にはそのどちらも正解です。とはいえ実際には意図した肘の位置もあるので、どちらか解を選択する必要があるのです。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "> 余談：\n", "> ロボットの「肘の位置」は大きな問題となることがあります。今回は地面が障害になりましたが、狭い空間にロボットアームを伸ばして作業する場合、手先に気を使うと同時に肘も障害物に当たらないようにしなければなりません。今回は2自由度のロボットを考えましたが、同じ上下の軸に3以上のさらに冗長な自由度を持ったロボットを設計することも可能です。このようなロボットは同じ手先座標でも肘の位置をかなり自由に選択することが可能です。計算がめんどうくさくなる、という一方で、狭い空間でも肘の位置をコントロールして障害物を避けながら進むことができるため面白いロボットを作ることができます。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "> 余談２：\n", "> とは言っても、冗長な自由度を持ったロボットは計算が大変なだけでなく、長尺の構造材の歪や各モータ部分でガタ（バックラッシュ）が発生し誤差として蓄積するため、手先座標の精度の確保が難しくなります。実用上は、自由度が高いと同時に精度を出しやすい設計のロボットが多用されます。「スカラ型ロボット」はそのようなロボットの一つで、xy軸は今回計算したものと同じ2自由度の構造をしているのですが、手先近くにz軸方向の直動関節があります。機械的な剛性が確保しやすく、計算し易さと精度の高さを両立できる優れた設計のロボットです。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 数値的に求める方法" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "「解析的な方法」では数学的に逆関数を求めていきましたが、今回のような大幅に簡略化したロボットであってもかなり骨が折れる作業だったと思います。より多くの関節を持った複雑なロボットの関節角を計算する場合は更に大変な作業になります。\n", "\n", "解析的な方法では関節角を逆関数として求めようとしましたが、逆関数を用いず、コンピュータを使った繰り返し計算によって関節角を求めることもできます。そのような方法を「数値的な方法」と呼びます。ここではニュートン法を使った例を紹介します。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "ニュートン法では以下の考え方で解に近づいていきます。\n", "\n", "まずは、関節角に適当な初期値を与えます。「適当」は案外難しいのですが、ここではサイコロを振って決めたランダムな値と考えてください。\n", "\n", "$$ \\theta_1 = 初期値 $$\n", "\n", "関節の初期値から順運動学の式を使って手先座標を求めます。\n", "\n", "$$ r_1 = f(\\theta_1) $$\n", "\n", "ランダムに決めた初期値がたまたま正解だった場合を除いて、この手先座標は目標座標とは異なっているはずです。\n", "\n", "目標座標と現在の手先座標との差分を計算します。この差分値は、手先座標のxy軸の空間の中で現在の座標と目標座標がどちらの方向にどれだけ離れているかという指標になります。\n", "\n", "$$ r - r_1 $$\n", "\n", "手先座標のxy空間での指標が得られたとは言っても、今回は関節角の調整でその指標に近づいていかなければなりません。ニュートン法ではここで関数の微分を使います。順運動学の微分の式に対して現在の関節角の値を代入すると「関節角をどれだけ動かすと手先座標のxy軸の空間で手先がどれだけ動くか」という傾きが定数として得られます。\n", "\n", "$$ A_1 = f'(\\theta_1) $$\n", "\n", "元の順運動学の式が三角関数なので本来この傾きは関節角に応じて複雑な動きをするはずです。なので、ここで得られた定数は「現在の関節角の付近ではこの傾き」という暫定的な値を切り出したものでしかありません。ですが近似としては十分使えます。\n", "\n", "上記の傾きを手先座標のxy空間と関節角をつなぐ「のり」のように使います。\n", "\n", "式の左辺を手先座標のxy空間、右辺を関節角の空間と考えて、のりでつなぎ合わせます。ここで$r$と$\\theta$はそれぞれ手先座標と関節の目標値です。\n", "\n", "$$ r - r_1 = A_1 (\\theta - \\theta_1) $$\n", "\n", "厳密にはこのような等式は成立するわけではなく、今この瞬間に限っては近似的にこのような関係が成り立つと考えよう、と信じるわけです。\n", "\n", "上記の式を$\\theta$に関して解くと以下のようになります。\n", "\n", "$$ \\theta = \\theta_1 + A_1^{-1} (r - r_1) $$\n", "\n", "$r_1$と$A_1$は順運動学から求めたので置き換えると、目標座標$r$と関節角の初期値$\\theta_1$から正しい関節角$\\theta$を求める以下の式になります。\n", "\n", "$$ \\theta = \\theta_1 + f'(\\theta_1)^{-1} (r - f(\\theta_1)) $$\n", "\n", "$f'(\\theta_1)$の値に関してはあくまで$\\theta_1$付近での近似にすぎないので、実際は以下のようにして何度も繰り返し計算して精度を高めます。\n", "\n", "$$ \\theta_{i+1} = \\theta_i + k f'(\\theta_i)^{-1} (r - f(\\theta_i)) $$\n", "\n", "ここで$k$は$A=f'(\\theta)$が近似としてあまり信用できない場合に使う手加減の係数です（今回もその信用できない場合にあたります）。この係数を小さめにすることで、小刻みに解に近づこうとするため計算量は増えますが収束がより確実になります。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "ニュートン法では、関数の微分のみを使って解を求められるのがポイントです。複雑な変換が必要な逆関数とは異なり微分は簡単に導出することができます。" ] }, { "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": [ "適当な初期値から$A$と$A^{-1}$を求めてみましょう。" ] }, { "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}$は$A$の逆関数ですが、$A$は定数なので簡単に求まります。$A$が1次元であれば\n", "\n", "$$A^{-1} = \\frac{1}{A}$$\n", "\n", "ですが、ここでは式の入出力がそれぞれ$r = (x, y)$、$\\theta = (\\theta_1, \\theta_2)$の2次元であるため、$A$も2行2列の行列になります。2行2列の行列の逆関数（逆行列）は以下のように計算できます。\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", "一見複雑な式に見えますが、各行列要素の掛け算と割り算だけなので一瞬で計算できます。" ] }, { "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": [ "ここでは逆行列の計算に通常のinv()ではなく疑似逆行列を計算するpinv()を用いています。今回は深く説明しませんが、擬似逆行列を使うことでロボットの手が伸びきった状態などでの計算の安定性が向上します。\n", "\n", "繰り返し計算で解を求めるプログラムを書いてみましょう。" ] }, { "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": [ "グラフを描いて確認しましょう。" ] }, { "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": [ "解を正しく推定できているようです。\n", "\n", "数値的な方法を使うことで逆関数を導出することなく逆運動学を計算することができました。\n", "\n", "ここで利用したニュートン法は、逆関数を導出することが難しい複雑な関数であっても近似的にパラメータを推定することができる非常に強力なアルゴリズムで、ロボットの制御にとどまらず様々な最適化計算に用いられています。\n", "その一方で収束性が保証されていなかったり、初期値をうまく取らないと解が発散するという使用上の注意点もありますので、それらの特性を理解した上でうまく利用する必要があります。\n", "\n", "数値的な方法は利用上の注意点がいくつかあるとはいえ、解析解を得ることが難しい複雑な構造のロボットであっても多少の繰り返し計算のみで容易に逆運動学を導出できます。近年のコンピュータの高速化は著しく、小型の組込みプロセッサでも逆運動学の計算が実用的な速度で動いてしまうため、数値的な方法は高度なロボットシステムで多く用いられています。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# ロボットへの実装" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Pythonによるプログラミングかと思いきや実際は数式の変換の連続で嫌になってきたころだと思いますが、それもここまでです。\n", "\n", "いよいよ上記の逆運動学をC++でプログラミングしてロボットを動かしてみます。\n", "\n", "[前回の記事](http://tech.mid-japan.com/blog/2014/07/10/learn-pid-control-and-rtm/)でPA10ロボット用のPID制御コントローラを実装して調整しました。また、RTMを使うとシステムを「コンポーネント」という部品に分割して開発できることを知りました。今回は前回開発したPID制御コントローラを組み替えることで以下の設計のシステムを組んでみます。" ] }, { "cell_type": "code", "execution_count": 31, "metadata": { "collapsed": false }, "outputs": [ { "data": { "image/svg+xml": [ "\n", "\n", "\n", "\n", "\n", "\n", "G\n", "\n", "\n", "手先指令\n", "\n", "手先指令\n", "\n", "\n", "逆運動学\n", "\n", "逆運動学\n", "\n", "\n", "手先指令->逆運動学\n", "\n", "\n", "手先位置\n", "\n", "\n", "PID制御\n", "\n", "PID制御\n", "\n", "\n", "逆運動学->PID制御\n", "\n", "\n", "各関節角\n", "\n", "\n", "PA10\n", "\n", "PA10\n", "\n", "\n", "PID制御->PA10\n", "\n", "\n", "各関節トルク\n", "\n", "\n", "PA10->PID制御\n", "\n", "\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", " 手先指令 -> 逆運動学 [label=\"手先位置\"]\n", " 逆運動学 -> PID制御 [label=\"各関節角\"]\n", " PID制御 -> PA10 [label=\"各関節トルク\"]\n", " PA10 -> PID制御 [label=\"各関節角\"]\n", "}\n", "''')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "今回のメインは図の真ん中にある「逆運動学コンポーネント」です。\n", "\n", "ここまでの作業で解析的な方法と数値的な方法の両方で逆運動学を導出しましたが、大変だったのは解析的な方法の方だったと思います。せっかく頑張って数式を変換したのでここでは解析的な方法で実装してみましょう。\n", "\n", "解析的な方法では最終的に以下の式が得られました：" ] }, { "cell_type": "code", "execution_count": 32, "metadata": { "collapsed": false }, "outputs": [ { "data": { "image/png": "iVBORw0KGgoAAAANSUhEUgAAAqAAAAAlCAYAAACd3luHAAAABHNCSVQICAgIfAhkiAAAFcJJREFU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\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": [ "この式をC++で実装すると以下のようになります。\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": [ "ソースコード全体を以下のレポジトリで公開しています。\n", "\n", "https://github.com/devrt/inverse-kinematics-primer\n", "\n", "InverseKinematics.cpp, HandMotionGeneration.cpp, PIDController.cppが各コンポーネントのソースコードになっているので、まずは読んでみてください。\n", "\n", "シミュレーションを起動するにはhrpsys-baseインストール済みのLinux環境が必要です。\n", "[数回前の記事](tech.mid-japan.com/blog/2014/06/28/vagrant-hrpsys-watchdog/)で紹介したVagrantを使うと環境を気軽に立ち上げることができるので、特にWindowsを使っている方にはお勧めです。\n", "\n", "環境を立ち上げ、ログインしたらレポジトリをチェックアウトしてスクリプトを起動します。\n", "\n", "```\n", "git clone https://github.com/devrt/inverse-kinematics-primer.git\n", "cd inverese-kinematic-primer\n", "./run.sh\n", "```\n", "\n", "ディフォルトの状態では手先に三角の軌跡を描かせるプログラムが入っています。" ] }, { "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": [ "問題なく動作しているようです。\n", "\n", "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": [ "指を開け閉めできないのが無念、、、（頑張れば箱を取ることもできるので、この先は各自ソースコードをいじってみてください）。" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# おまけ" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "今回は手を動かしながら一から式を導出＆実装していきましたが、逆運動学はロボットの制御で多用されるため様々なライブラリが開発されています。\n", "\n", "現在、ロボット界で最も人気があるのはRosen Diankovによって開発されているIKFastです。\n", "\n", "http://openrave.org/docs/latest_stable/openravepy/ikfast/\n", "\n", "IKFastはロボットの構造を与えると自動で解析解を導出（内部でsympyを利用）して、解けた式を自動でC++のコードに変換して出力してくれます。\n", "\n", "解析解を使っているため、逆運動学の結果は安定しており計算も非常に高速です。\n", "\n", "ROSからIKFastを利用するには以下のパッケージを使います。\n", "\n", "http://moveit.ros.org/wiki/Kinematics/IKFast\n", "\n", "RTMからOpenRAVE（IKFastの親プロジェクト）を利用することができるパッケージが、岡田先生（玉川大学）により開発＆公開されています。\n", "\n", "https://code.google.com/p/rtc-openrave/\n", "\n", "\n", "OpenHRPにも逆運動学を計算するライブラリがあります。\n", "\n", "こちらは数値的な方法で計算するものなので安定性については注意が必要ですが、シミュレーションに使うのと同じVRML形式のモデルファイルから逆運動学を計算できるので非常に便利です。\n", "\n", "http://www.openrtp.jp/openhrp3/jp/reference/idl/html/interfaceOpenHRP_1_1DynamicsSimulator.html\n", "\n", "これを使うと以下のビデオのようなデモを作ることができます（デモに使ったソースコードは以下を参照）。\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 }