2DoF_forward_kinematics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 2DoF forward kinematics\n",
    "2自由度マニピュレータの順運動学です。\n",
    "\n",
    "## Denavit-Hartenberg parameters\n",
    "\n",
    "|$^{i-1}T_i$|$a_i$|$\\alpha_i$|$d_i$|$\\theta_i$|\n",
    "|---|---|---|---|---|\n",
    "|$^{B}T_1$  |0    |0         |0    |$\\theta_1$|\n",
    "|$^{1}T_2$  |1    |0         |0    |$\\theta_2$|\n",
    "|$^{2}T_E$  |1    |0         |0    |0         |"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Homogeneous transformation matrix\n",
    "# 同次変換行列\n",
    "def trans_m(a, alpha, d, theta):\n",
    "    m = np.array([[np.cos(theta), -np.sin(theta), 0, a],\n",
    "                  [np.cos(alpha)*np.sin(theta), np.cos(alpha)*np.cos(theta), -np.sin(alpha), -np.sin(alpha)*d],\n",
    "                  [np.sin(alpha)*np.sin(theta), np.sin(alpha)*np.cos(theta),  np.cos(alpha),  np.cos(alpha)*d],\n",
    "                  [0, 0, 0, 1]])\n",
    "    return m"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 関節角の指定\n",
    "theta = np.array([[np.pi/6],\n",
    "                  [np.pi/2]])\n",
    "\n",
    "# DHパラメータより同次変換行列を求める\n",
    "tm0_1 = trans_m(0, 0, 0, theta[0,0])\n",
    "tm1_2 = trans_m(1, 0, 0, theta[1,0])\n",
    "tm2_3 = trans_m(1, 0, 0, 0)\n",
    "\n",
    "# 基準座標系から見た各座標系の位置姿勢を計算\n",
    "r = [tm0_1[0:2,3:4],\n",
    "     tm0_1.dot(tm1_2)[0:2,3:4],\n",
    "     tm0_1.dot(tm1_2).dot(tm2_3)[0:2,3:4]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# マニピュレータの描画\n",
    "def draw(r):\n",
    "    xs = [e[0] for e in r]\n",
    "    ys = [e[1] for e in r]\n",
    "    \n",
    "    fig = plt.figure(figsize=(5,5))\n",
    "    sp = fig.add_subplot(111,aspect='equal')\n",
    "    sp.set_xlim(-2,2)\n",
    "    sp.set_ylim(-2,2)\n",
    "    plt.plot(xs, ys, \"-\", color=\"skyblue\", lw=5, label=\"link\")\n",
    "    plt.plot(xs, ys, \"o\", color=\"orange\", ms=10, label=\"joint\")\n",
    "    plt.grid()\n",
    "    plt.legend(fontsize=10)\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 360x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "r: [[0.3660254 1.3660254]]\n"
     ]
    }
   ],
   "source": [
    "draw(r)\n",
    "print(\"r: \"+str(r[2].T))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}