{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Cinématique & espace de travail d'un manipulateur 2 axes"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"source : https://sajidnisar.github.io/posts/python_kinematics_dh"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Objectif\n",
"\n",
"Ce document fournit une base pour dériver et analyser la cinématique de manipulateurs robotiques séries en utilisant Python. L'objectif est de fournir un programme d’apprentissage, de calcul et de compréhension de la cinématique de robots série de manière agréable et interactive.\n",
"\n",
"Nous utilisons l’environnement Jupyter Notebook \\ Matplotlib qui offre un moyen flexible d’interagir avec le code et d’afficher des tracés en ligne. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Prérequis\n",
"\n",
"**Côté technique**: la connaissance de la cinématique robot est recommandée. Pour une utilisation avancée, par exemple pour étendre cet exemple à des conceptions plus complexes, il est nécessaire de connaître la notation DH (Denavit Hartenberg classique). \n",
"\n",
"**Côté programmation** : des compétences de niveau débutant à intermédiaire avec Python et Jupyter notebook sont souhaitables. Les bibliothèques python utilisées sont:\n",
"\n",
" Sympy (pour le calcul symbolique)\n",
" Numpy (pour la calcul numérique)\n",
" Matplotlib (pour les tracés)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Configuration de l'environnement\n",
"\n",
"Nous importons ici la bibliothèque Sympy qui nous permettra de développer et de manipuler des expressions symboliques."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import sympy as sp\n",
"%matplotlib notebook"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Dans SymPy, nous initialisons l'impression afin que toutes les expressions mathématiques puissent être rendues en notation mathématique standard."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from sympy.physics.vector import init_vprinting\n",
"init_vprinting(use_latex='mathjax', pretty_print=False)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Le manipulateur plan 2 axes "
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from IPython.display import Image\n",
"Image('fig/2rp_new.png', width=300)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Pourquoi le manipulateur 2RP ?\n",
"\n",
"Presque tous les manuels de robotique commencent à enseigner la cinématique des robots à l'aide d'un exemple simple et fondamental : le *manipulateur plan à 2 axes* (2RP, R pour les liaisons *revolute* (-> rotoïde) et P le plan). Par souci de simplicité et comme point de départ, nous commençons également par l’exemple de ce manipulateur, qui devrait être facile à suivre et à comprendre.\n",
"\n",
"Maintenant, nous déclarons les symboles (longueur des liens, variables articulaire etc.) qui seront utilisés par la suite."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from sympy.physics.mechanics import dynamicsymbols"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"theta1, theta2, l1, l2, theta, alpha, a, d = dynamicsymbols('theta1 theta2 l1 l2 theta alpha a d')\n",
"theta1, theta2, l1, l2, theta, alpha, a, d"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Cinématique utilisant la méthode DH\n",
"\n",
"Nous définissons les repères (*frame*) de notre manipulateur conformément à la convention classique DH. La méthode DH offre un moyen pratique d’exprimer la position et l’orienation d’un corps rigide sous forme de matrice en utilisant les longueurs de liaison, les angles de liaison, les angles de torsion et les décalages de liaison appropriés.\n",
"\n",
"### Table DH\n",
"\n",
"La table DH pour la manipulateur plan 2 axes décrit par la figure ci-dessous\n",
"\n",
""
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$i$: numéro corps , $\\alpha_i$: angle de torsion, $a_i$: longeur de la liaison $i$, $d_i$: offset liaison et $\\theta_i$: angle de la liaison."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Transformation Homogène"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"La matrice de transformation homogène standard est représentée par :
\n",
"(cf https://en.wikipedia.org/wiki/Denavit-Hartenberg_parameters)\n",
""
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"soit, avec les notations adoptées :"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"R = sp.Matrix( )\n",
"R"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"T = sp.Matrix( )\n",
"T"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"last_row = sp.Matrix([[0, 0, 0, 1]])\n",
"last_row"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"M = sp.Matrix.vstack(sp.Matrix.hstack(T, T), last_row)\n",
"M"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Transformation du repère $R_0$ au repère $R_1$\n",
"\n",
"En déduire $M_{01}$, la transformation du repère $R_0$ au repère $R_1$ :
\n",
"*indication* : substituer respectivement *alpha*, *a*, *theta* et *d* dans M par *0*, *l1*, *theta1* et *0*"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"M01 = M.subs({ })\n",
"M01"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Transformation du repère $R_1$ au repère $R_2$\n",
"\n",
"En déduire $M_{12}$, la matrice de transformation homogène du repère $R_1$ au repère $R_2$ :"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"M12 = M.subs({ )\n",
"M12"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Matrice de transformation homogène du repère $R_0$ au repère $R_2$\n",
"\n",
"En déduire la matrice $M_{02}$ de transformation homogène du repère $R_0$ au repère $R_2$ :"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"M02 = \n",
"M02"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Simplifier la matrice $M_{02}$ avec `sp.simplify` :"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"M02 = \n",
"M02"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Évaluation de la position de l'outil"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Déduire la position du poignet dans la direction x :"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"px = \n",
"px"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Déduire la position du poignet dans la direction y :"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"py = \n",
"py"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Pour évaluer numériquement la position de la pointe, utiliser la fonction `lambdify` de Sympy avec les arguments *l1*, *l2*, *theta1* et *thetat2* :"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"fx = sp.lambdify((l1, l2, theta1, theta2), px)\n",
"fy = sp.lambdify((l1, l2, theta1, theta2), py)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Espace de travail du manipulateur"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"En prenant $l_1 = 50\\ mm$ et $l_2 = 20\\ mm$ tracer les courbes des fonctions `fx` et `fy` pour *theta1* et *theta2* variant de 0 à $2\\,\\pi$ :"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"%matplotlib inline\n",
"\n",
"import numpy as np\n",
"d2r = np.deg2rad\n",
"\n",
"fig = plt.figure()\n",
"ax = fig.add_subplot(111)\n",
"\n",
"theta11 = np.linspace(d2r(0),d2r(360),100)\n",
"theta22 = np.linspace(d2r(0), d2r(360),100)\n",
"theta1, theta2 = np.meshgrid(theta11, theta22)\n",
"\n",
"l1, l2 = 50, 20\n",
"X = np.array(fx(l1, l2, theta1, theta2))\n",
"Y = np.array(fy(l1, l2, theta1, theta2))\n",
"\n",
"ax.plot(X, Y, 'b')\n",
"plt.grid()\n",
"plt.axis('equal')\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# This import registers the 3D projection, but is otherwise unused.\n",
"from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import\n",
"import matplotlib.pyplot as plt\n",
"from matplotlib import cm\n",
"import numpy as np\n",
"d2r = np.deg2rad\n",
"\n",
"%matplotlib notebook\n",
"\n",
"fig = plt.figure()\n",
"ax = fig.add_subplot(111, projection='3d')\n",
"\n",
"theta11 = np.linspace(d2r(0),d2r(360),100)\n",
"theta22 = np.linspace(d2r(0), d2r(360),100)\n",
"theta1, theta2 = np.meshgrid(theta11, theta22)\n",
"\n",
"l1, l2 = 50, 20\n",
"\n",
"X = np.array(fx(l1, l2, theta1, theta2))\n",
"Y = np.array(fy(l1, l2, theta1, theta2))\n",
"Z = Y*0\n",
"\n",
"ax.plot_surface(X, Y, Z, shade=False,cmap=cm.coolwarm )\n",
"\n",
"ax.set_xlabel('X')\n",
"ax.set_ylabel('Y')\n",
"ax.set_zlabel('Z')\n",
"\n",
"plt.show()\n"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.7"
}
},
"nbformat": 4,
"nbformat_minor": 2
}