{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "slide"
}
},
"source": [
"# Kinematic chain in a plane (2D)\n",
"\n",
"> Marcos Duarte, Renato Naville Watanabe \n",
"> Laboratory of Biomechanics and Motor Control ([http://pesquisa.ufabc.edu.br/bmclab](http://pesquisa.ufabc.edu.br/bmclab)) \n",
"> Federal University of ABC, Brazil"
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "skip"
}
},
"source": [
"Kinematic chain refers to an assembly of rigid bodies (links) connected by joints that is the mathematical model for a mechanical system which in turn can represent a biological system such as the human arm ([Wikipedia](http://en.wikipedia.org/wiki/Kinematic_chain)). \n",
"\n",
"The term chain refers to the fact that the links are constrained by their connections (typically, by a hinge joint which is also called pin joint or revolute joint) to other links. As consequence of this constraint, a kinematic chain in a plane is an example of circular motion of a rigid object. \n",
"\n",
"Chapter 16 of Ruina and Rudra's book is a good formal introduction on the topic of circular motion of a rigid object. However, in this notebook we will not employ the mathematical formalism introduced in that chapter - the concept of a rotating reference frame and the related rotation matrix - we will do that in a future notebook when talking about rigid body transformation. For now we will specify the kinematics in a Cartesian coordinate system using trigonometry and calculus. This approach is simpler and more intuitive but it gets too complicated for a kinematic chain with many links or in the 3D space. For such more complicated problems, it would be recommended using rigid transformations (see for example, Siciliano et al. (2009)). \n",
"\n",
"We will deduce the kinematic properties of kinematic chains algebraically using [Sympy](http://sympy.org/), a Python library for symbolic mathematics. With Sympy, we could have used the [mechanics module](http://docs.sympy.org/latest/modules/physics/mechanics/index.html), a specific module for creation of symbolic equations of motion for multibody systems, but let's deduce most of the stuff by ourselves to understand the details."
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "skip"
}
},
"source": [
"## Properties of kinematic chains\n",
"\n",
"For a kinematic chain, the base is the extremity (origin) of a kinematic chain which is typically considered attached to the ground, body or fixed. The endpoint is the other extremity (end) of a kinematic chain and typically can move. In robotics, the term end-effector is used and usually refers to a last link (rigid body) in this chain. \n",
"\n",
"In topological terms, a kinematic chain is termed open when there is only one sequence of links connecting the two ends of the chain. Otherwise it's termed closed and in this case a sequence of links forms a loop. A kinematic chain can be classified as serial or parallel or a mixed of both. In a serial chain the links are connected in a serial order. A serial chain is an open chain, otherwise it is a parallel chain or a branched chain (e.g., hand and fingers). \n",
"\n",
"Another important term to characterize a kinematic chain is degree of freedom (DOF). In mechanics, the degree of freedom of a mechanical system is the number of independent parameters that define its configuration or that determine the state of a physical system. A particle in the 3D space has three DOFs because we need three coordinates to specify its position. A rigid body in the 3D space has six DOFs because we need three coordinates of one point at the body to specify its position and three angles to to specify its orientation in order to completely define the configuration of the rigid body. For a link attached to a fixed body by a hinge joint in a plane, all we need to define the configuration of the link is one angle and then this link has only one DOF. A kinematic chain with two links in a plane has two DOFs, and so on.\n",
"\n",
"The mobility of a kinematic chain is its total number of degrees of freedom. The redundancy of a kinematic chain is its mobility minus the number of degrees of freedom of the endpoint."
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "slide"
}
},
"source": [
"## The kinematics of one-link system\n",
"\n",
"First, let's study the case of a system composed by one planar hinge joint and one link, which technically it's not a chain but it will be useful to review (or introduce) key concepts. \n",
"
\n",
"\n",
"\n",
"First, let's import the necessary libraries from Python and its ecosystem:"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"slideshow": {
"slide_type": "skip"
}
},
"outputs": [],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"%matplotlib inline\n",
"import seaborn as sns\n",
"sns.set_context(\"notebook\", font_scale=1.2, rc={\"lines.linewidth\": 2,\n",
" \"lines.markersize\": 10})\n",
"from IPython.display import display, Math\n",
"\n",
"from sympy import Symbol, symbols, Function, Matrix, simplify, lambdify, expand, latex\n",
"from sympy import diff, cos, sin, sqrt, acos, atan2, atan\n",
"from sympy.vector import CoordSys3D\n",
"from sympy.physics.mechanics import dynamicsymbols, mlatex, init_vprinting\n",
"init_vprinting()\n",
"\n",
"import sys\n",
"sys.path.insert(1, r'./../functions') # add to pythonpath"
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "slide"
}
},
"source": [
"We need to define a Cartesian coordinate system and the symbolic variables, $t$, $\\ell$, $\\theta$ (and make $\\theta$ a function of time):"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"slideshow": {
"slide_type": "fragment"
}
},
"outputs": [],
"source": [
"G = CoordSys3D('')\n",
"t = Symbol('t')\n",
"l = Symbol('ell', positive=True)\n",
"theta = dynamicsymbols('theta') # or Function('theta')(t)"
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "slide"
}
},
"source": [
"Using trigonometry, the endpoint position in terms of the joint angle and link length is:"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"slideshow": {
"slide_type": "fragment"
}
},
"outputs": [
{
"data": {
"image/png": "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\n",
"text/latex": [
"$\\displaystyle (\\ell \\cos{\\left(\\theta{\\left(t \\right)} \\right)})\\mathbf{\\hat{i}_{}} + (\\ell \\sin{\\left(\\theta{\\left(t \\right)} \\right)})\\mathbf{\\hat{j}_{}}$"
],
"text/plain": [
"(ell*cos(theta(t)))*.i + (ell*sin(theta(t)))*.j"
]
},
"execution_count": 3,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"r_p = l*cos(theta)*G.i + l*sin(theta)*G.j\n",
"r_p"
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "fragment"
}
},
"source": [
"With the components:"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"slideshow": {
"slide_type": "fragment"
}
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAOcAAAAmCAYAAAA/QIwxAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAJ1klEQVR4Ae2ci5EUNxCGuSsCAJzBOQOMIzDOwMYRGDKAIgIXzsCQAXYGkAGGDIAIbC4D/H2Demoe0u7s7Myxc1ZX6WYktVqtv9V6jfbOPn/+fKNEZ2dnD8n7mfAEvnclvppeEagITEcg+dWPlPgDv3pdKnkzl0HhC9L/JNwiPKqOmUOpplUE5iGAPz3Hx/StVzz/Iu4EOKIzMnqJqdBHEv8mT++uVBGoCKyAQJoE3yL6A7723bCK82EC8WcEvTrrzcGfnDii9blhBKotv47xcMgP1Pwr4S42+GmoRc45H8D0moKXQ+aIh8fzfFsNG6hs81lt+XXthp/9lTQYrVJzzinvLse8T75TsTzOsB+TgXmttCUEsFu15WkYTF+6M1Sl5JxDviaeZslXRH5zjUz4lvfnBJ210oYQqLY8fWPlDoQ+obbL2uyeU6OS15tZc2mn3/SqYc5uubSK1LoIgHnW5w6aOVVx6JiltHWbU6UvgUC15RIorifjYOcMVfB2v9G8J7hvuRKiroeEx1dS2RGVoOOFupZEkHfLfMLohM40wkWp7NLp1kX4RNCWniFcCVHXSdsS/WbZkHKL2W+Wc6KADmmwE/npZXWizj9SJX7ALXb81RXZUwG62cG9+eFefEQJOy94vDSz066Gl3Ke3j0j/aoc9An1qbP1eVK/OnXaPMuWYkNwQFmlHyB3tg0XtR/CXKq2Acu4/v2zmzZ8J1/lvb1gsCO25dd4pw5nmLfKhqz7pzXqWUKmehIucrJIv0sQX/ftDWa826g2brpxQtPe4FvrST12cCs13F2rnpBLHUfbUj2Tvs9C7pJPZB9lQ8ofZD/4sz43a+YEiEsEeqPhR94f8VybXlDBb6mSezxP8p5vGsm9WeXH5Rw5Y3pPWfy61NsapPzXyFt9CU9dzvAe/mnLq8D1aFuqJ8HDTGf9RWkJG6KX9j3efgjqzXoIdQTdOXMOy6wZRxdH9k9RB++rjJYh/5gnur0nlGbNXjuiHvjF+2HE40mao2/b7kjf8tN2dtvE+8nZEp0WsSFyJttPTAgjnyvNnP/CXCRGl8+d4GywJjkzx/7MfZHgnRyBh7o50JVmTUf5ph2hPGVcnkkjvJHj6PsBnt6s2nAv9EfZhK4t1z4/WMSW6Oye0ytvgd8iiChXQUvY8ED72Wd05h7d7MZQLk4PvWiwi7x8oFMeBA7ydSyXJNlvqMMKE1jWEUtal4QaOEvw20A7mB1bMv6KMnFFygMY054S/iFI3xDkaX+6A4916hSCdodgmV/gGV1OJj1I7FoZkegTecrS8MOBLBwv9JW9S8oTq6zcLuOcd9uMbtpSezcdc6ocyq1my134k6eeHg6KnXZt+lIq45LZfAfB4OH1xveEN7T3dyM7aGkbTrWffI9pQ/8OAQq3y1oYBNyO2qaV3uGz8ZOXwPDawZ2+Jx90wOueyzosK3DFAwvyNIry28Mi+VNas9RMcdvYW3oSt3M+tq2QdY0wIO19CYtUThmj5WnK0ykbrHj6HkF9YcnjTZ7LwMl4leTsS6cOdVe/SctM+FazZZK9F3/4PLQZLQVJa/owz8aeth2yb/jSs/sQF/IXtSHyJtkPvsCzd7h6kwxHdpW3w/jTldEFXHmOJeReIuP2gXLUJWavO8hoZ8CMnNC/y2O7JBsvyePv54ZLT5ecXuJ3BHOmvDcaxb4MRmQVybpGy9PE7SAxWjFQhx1m1yGM8kL3JOrrP1a25T1aOAX/Etba9h46tv1Ye4O1wDnbZj9xmQktbcNJ9hNP9HNV5t0BHbRZHZ4TcUZyFPK+bNsg4qdAgiXYzoZFUGmDHVgH+JvQEmV0xNuEd/CYr7w3LUN6MT+9unTVQQXV72iC5XLDEXffkkindgDKkfUqt6Wkj/FYsrd5nRfbbtnrQJNseQT+XYyGg2/k7RvolrbhZPvRbidGtxheBrHvXZwTsdMYHpnA85RIfS4nOEbo7ZKmRMFTciDL6cCSo5jOqLHcwwqYy/iDiXLRIYaDgqOjbevO9AfL31CBqba0ScfiX5pVZ8F1VTakHidK+8VLnfWcP3YQN9Uu+a70Oh71FQlFw5mmfMuKkdKRp0TBE86S43P509QLJh4++csb10MC5nWzOMDJld23hBkOCu5Hds2a1uHgEHob3yQFpii/15bBOwP/JbBZ2oaT7Ue7PV95QfiZttvfbpz7RyLBZaPBpdyuDiz7qpQM1ChIRXtHQXS3478juF8ZEfJcFpsv32jpTn7MmA5QOqeO01LCxhkuBow2r/OiE2mMHiXdeo5JfRrCZcy+pbL19cr2hG8gcqgtadJc/JdAY2kbTrIfGDnou0JzQrCfNnQeL+kZI1uvcw54ZkVRwMverqXd3+4j93jq4nLbBjZEWb9vOfXnyNnf/J7uxG10NPgH3h+QFs4YchyxfqfO2Bc+hedWZKan8cgfZDVR6yh9anlJXlMncm2PA89okCBtSMor1qmOBL9TTsF0KHt2PNW7pi2n4j+00ew2pYJL23Cn/TrK2tcdrPvnKiT0PpvA5PH+ziN18j1l7IZJN1ko455wdPw91CHi8DqiuNfTCDrlPr3kc/azjLOTjtk7PjdOMN88g+/3B3WarpMrw6fxlid4u0/zCdnPLaSrV+jk0+9ZPdxzcfh0uuLnI8tAYmqYJLNQz0GfUjr1Lm5L2hGzSBZ/8h3ktHH0P9/FN5dumv0m2mffLupM3qI2RN5e+yUs1Wv8+WhoLJh2NmDIv3YcfQRYQ/WcbO1658hHR51kET2RY4fLOvtQN/jsgJOdM8lu9SQenXfn4Des99A49Zy8LdFxERsmjKfaL+tzw2UtMk+L6AB+H3xO2MLBiDNsbA2OBfIpApxlp9D34HM5hTHxfOTp4Z/OIsXW4c2X6Dp/N2LLpWx4iP2ygJ+8c2a1PtFEBxFUc98bnX2Wpu7pKOgyet+BUVxH/OfAipQv3UkOqr7uef4vn3Waxuf+LGHDQ+yX0yHSqnMGEss9PZiaOuOVanUfpZwp9GCKEw8EeRjl4Yd6WpeDSukwi6z/HR1rw0PsVwT34H/wVZRUM1oE0szpzNc/fWs5yi+UdX/tP1jbwjK+3JCN58y14Rz7UcY95+if6lXn3HgnqupvH4GSc9Zl7fZtW1twTRGoznlNDVubtX0EqnNu34a1BdcUgZxzehAR37+uabNrsyoCp4EA+00/axlGB4A55/R43W91XmWqVBGoCKyLgJcVpNHnt5FzpuN/P0b765TSJfNGWv1TEagIzEcA//I2kve3/ZnYaOYcfUqJqigYF74t9AOFLyOvPisCFYH5COBbbhu9qCDpmPGrqS8p6W/ROYMLQS5v/UfJ1TkDlPqsCByBQHJOf5WUdcoQ/R+PKTvrz+cPrAAAAABJRU5ErkJggg==\n",
"text/latex": [
"$\\displaystyle \\left\\{ \\mathbf{\\hat{i}_{}} : \\ell \\operatorname{cos}\\left(\\theta\\right), \\ \\mathbf{\\hat{j}_{}} : \\ell \\operatorname{sin}\\left(\\theta\\right)\\right\\}$"
],
"text/plain": [
"{_i: ell⋅cos(θ), _j: ell⋅sin(θ)}"
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"r_p.components"
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "slide"
}
},
"source": [
"### Forward and inverse kinematics\n",
"\n",
"Computing the configuration of a link or a chain (including the endpoint location) from the joint parameters (joint angles and link lengths) as we have done is called [forward or direct kinematics](https://en.wikipedia.org/wiki/Forward_kinematics).\n",
"\n",
"If the linear coordinates of the endpoint position are known (for example, if they are measured with a motion capture system) and one wants to obtain the joint angle(s), this process is known as [inverse kinematics](https://en.wikipedia.org/wiki/Inverse_kinematics). For the one-link system above:\n",
"\n",
"$$ \\theta = arctan\\left(\\frac{y_P}{x_P}\\right) $$"
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "slide"
}
},
"source": [
"### Matrix representation of the kinematics\n",
"\n",
"The mathematical manipulation will be easier if we use the matrix formalism (and let's drop the explicit dependence on $t$):"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"slideshow": {
"slide_type": "fragment"
}
},
"outputs": [
{
"data": {
"image/png": "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\n",
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}\\ell \\operatorname{cos}\\left(\\theta\\right)\\\\\\ell \\operatorname{sin}\\left(\\theta\\right)\\end{matrix}\\right]$"
],
"text/plain": [
"⎡ell⋅cos(θ)⎤\n",
"⎢ ⎥\n",
"⎣ell⋅sin(θ)⎦"
]
},
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"r = Matrix((r_p.dot(G.i), r_p.dot(G.j)))\n",
"r"
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "fragment"
}
},
"source": [
"We could have used Sympy for switching to matrix representation:"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {
"slideshow": {
"slide_type": "fragment"
}
},
"outputs": [
{
"data": {
"image/png": "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\n",
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}\\ell \\operatorname{cos}\\left(\\theta\\right)\\\\\\ell \\operatorname{sin}\\left(\\theta\\right)\\\\0\\end{matrix}\\right]$"
],
"text/plain": [
"⎡ell⋅cos(θ)⎤\n",
"⎢ ⎥\n",
"⎢ell⋅sin(θ)⎥\n",
"⎢ ⎥\n",
"⎣ 0 ⎦"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"r_p.to_matrix(G)"
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "fragment"
}
},
"source": [
"The third element of the matrix above refers to the $\\hat{\\mathbf{k}}$ component which is zero for the present case."
]
},
{
"cell_type": "markdown",
"metadata": {
"slideshow": {
"slide_type": "slide"
}
},
"source": [
"## Differential kinematics\n",
"\n",
"Differential kinematics gives the relationship between the joint velocities and the corresponding endpoint linear velocity. This mapping is described by a matrix, termed [Jacobian matrix](http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant), which depends on the kinematic chain configuration and it is of great use in the study of kinematic chains. \n",
"First, let's deduce the endpoint velocity without using the Jacobian and then we will see how to calculate the endpoint velocity using the Jacobian matrix.\n",
"\n",
"The velocity of the endpoint can be obtained by the first-order derivative of the position vector. The derivative of a vector is obtained by differentiating each vector component: \n",
"\n",
"$$\n",
"\\frac{\\mathrm{d}\\overrightarrow{\\mathbf{r}}}{\\mathrm{d}t} = \n",
"\\large\n",
"\\begin{bmatrix}\n",
"\\frac{\\mathrm{d}x_P}{\\mathrm{d}t} \\\\\n",
"\\frac{\\mathrm{d}y_P}{\\mathrm{d}t} \\\\\n",
"\\end{bmatrix}\n",
"$$\n",
"\n",
"Note that the derivative is with respect to time but $x_P$ and $y_P$ depend explicitly on $\\theta$ and it's $\\theta$ that depends on $t$ ($x_P$ and $y_P$ depend implicitly on $t$). To calculate this type of derivative we will use the [chain rule](http://en.wikipedia.org/wiki/Chain_rule). \n",
"\n",
"
\n",
"