From e754a8d9d8c537ea6b710af77ed5a8a198e7c9e4 Mon Sep 17 00:00:00 2001 From: Henrique Ferrolho Date: Sat, 23 Feb 2019 12:38:49 +0000 Subject: [PATCH] Adds Jupyter Notebook with examples --- examples/Modern Robotics Examples.ipynb | 2472 +++++++++++++++++++++++ 1 file changed, 2472 insertions(+) create mode 100644 examples/Modern Robotics Examples.ipynb diff --git a/examples/Modern Robotics Examples.ipynb b/examples/Modern Robotics Examples.ipynb new file mode 100644 index 0000000..5361302 --- /dev/null +++ b/examples/Modern Robotics Examples.ipynb @@ -0,0 +1,2472 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Modern Robotics\n", + "**Mechanics, Planning, and Control**" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "using Revise\n", + "using ModernRobotics\n", + "\n", + "import LinearAlgebra\n", + "const linalg = LinearAlgebra;" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## BASIC HELPER FUNCTIONS" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "true" + ] + }, + "execution_count": 2, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "NearZero(-1e-7)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "1×3 Array{Float64,2}:\n", + " 0.267261 0.534522 0.801784" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "Normalize([1 2 3])" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## CHAPTER 3: RIGID-BODY MOTIONS" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "scrolled": true + }, + "outputs": [ + { + "data": { + "text/plain": [ + "3×3 LinearAlgebra.Adjoint{Int64,Array{Int64,2}}:\n", + " 0 1 0\n", + " 0 0 1\n", + " 1 0 0" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "RotInv([0 0 1; 1 0 0; 0 1 0])" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3×3 Array{Int64,2}:\n", + " 0 -3 2\n", + " 3 0 -1\n", + " -2 1 0" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "VecToso3([1 2 3])" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3-element Array{Int64,1}:\n", + " 1\n", + " 2\n", + " 3" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "so3ToVec([0 -3 2; 3 0 -1; -2 1 0])" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "([0.267261 0.534522 0.801784], 3.7416573867739413)" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "AxisAng3([1 2 3])" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3×3 Array{Float64,2}:\n", + " -0.694921 0.713521 0.0892929\n", + " -0.192007 -0.303785 0.933192 \n", + " 0.692978 0.63135 0.348107 " + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "MatrixExp3([0 -3 2; 3 0 -1; -2 1 0])" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3×3 Array{Float64,2}:\n", + " 0.0 -1.2092 1.2092\n", + " 1.2092 0.0 -1.2092\n", + " -1.2092 1.2092 0.0 " + ] + }, + "execution_count": 9, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "MatrixLog3([0 0 1; 1 0 0; 0 1 0])" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4×4 Array{Int64,2}:\n", + " 1 0 0 1\n", + " 0 0 -1 2\n", + " 0 1 0 5\n", + " 0 0 0 1" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "RpToTrans([1 0 0; 0 0 -1; 0 1 0], [1, 2, 5])" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "([1 0 0; 0 0 -1; 0 1 0], [0, 0, 3])" + ] + }, + "execution_count": 11, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "TransToRp([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4×4 Array{Int64,2}:\n", + " 1 0 0 0\n", + " 0 0 1 -3\n", + " 0 -1 0 0\n", + " 0 0 0 1" + ] + }, + "execution_count": 12, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "TransInv([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4×4 Array{Float64,2}:\n", + " 0.0 -3.0 2.0 4.0\n", + " 3.0 0.0 -1.0 5.0\n", + " -2.0 1.0 0.0 6.0\n", + " 0.0 0.0 0.0 0.0" + ] + }, + "execution_count": 13, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "VecTose3([1 2 3 4 5 6])" + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "1×6 Array{Int64,2}:\n", + " 1 2 3 4 5 6" + ] + }, + "execution_count": 14, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "se3ToVec([0 -3 2 4; 3 0 -1 5; -2 1 0 6; 0 0 0 0])" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "6×6 Array{Float64,2}:\n", + " 1.0 0.0 0.0 0.0 0.0 0.0\n", + " 0.0 0.0 -1.0 0.0 0.0 0.0\n", + " 0.0 1.0 0.0 0.0 0.0 0.0\n", + " 0.0 0.0 3.0 1.0 0.0 0.0\n", + " 3.0 0.0 0.0 0.0 0.0 -1.0\n", + " 0.0 0.0 0.0 0.0 1.0 0.0" + ] + }, + "execution_count": 15, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "Adjoint([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])" + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "6-element Array{Int64,1}:\n", + " 0\n", + " 0\n", + " 1\n", + " 0\n", + " -3\n", + " 2" + ] + }, + "execution_count": 16, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "ScrewToAxis([3; 0; 0], [0; 0; 1], 2)" + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "([1.0, 0.0, 0.0, 1.0, 2.0, 3.0], 1.0)" + ] + }, + "execution_count": 17, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "AxisAng6([1, 0, 0, 1, 2, 3])" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4×4 Array{Float64,2}:\n", + " 1.0 0.0 0.0 0.0 \n", + " 0.0 6.7949e-9 -1.0 1.01923e-8\n", + " 0.0 1.0 6.7949e-9 3.0 \n", + " 0.0 0.0 0.0 1.0 " + ] + }, + "execution_count": 18, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "MatrixExp6([0 0 0 0; 0 0 -1.57079632 2.35619449; 0 1.57079632 0 2.35619449; 0 0 0 0])" + ] + }, + { + "cell_type": "code", + "execution_count": 19, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4×4 Array{Float64,2}:\n", + " 0.0 0.0 0.0 0.0 \n", + " 0.0 0.0 -1.5708 2.35619\n", + " 0.0 1.5708 0.0 2.35619\n", + " 0.0 0.0 0.0 0.0 " + ] + }, + "execution_count": 19, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "MatrixLog6([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3×3 Array{Float64,2}:\n", + " 0.679011 0.148945 0.718859\n", + " 0.373207 0.773196 -0.512723\n", + " -0.632187 0.616428 0.469421" + ] + }, + "execution_count": 20, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "ProjectToSO3([0.675 0.150 0.720; 0.370 0.771 -0.511; -0.630 0.619 0.472])" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4×4 Array{Float64,2}:\n", + " 0.679011 0.148945 0.718859 1.2\n", + " 0.373207 0.773196 -0.512723 5.4\n", + " -0.632187 0.616428 0.469421 3.6\n", + " 0.0 0.0 0.0 1.0" + ] + }, + "execution_count": 21, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "ProjectToSE3([0.675 0.150 0.720 1.2; 0.370 0.771 -0.511 5.4; -0.630 0.619 0.472 3.6; 0.003 0.002 0.010 0.9])" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "0.08835298523536149" + ] + }, + "execution_count": 22, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "DistanceToSO3([1.0 0.0 0.0 ; 0.0 0.1 -0.95; 0.0 1.0 0.1])" + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "0.13493053768513638" + ] + }, + "execution_count": 23, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "DistanceToSE3([1.0 0.0 0.0 1.2; 0.0 0.1 -0.95 1.5; 0.0 1.0 0.1 -0.9; 0.0 0.0 0.1 0.98])" + ] + }, + { + "cell_type": "code", + "execution_count": 24, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "false" + ] + }, + "execution_count": 24, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "TestIfSO3([1.0 0.0 0.0; 0.0 0.1 -0.95; 0.0 1.0 0.1])" + ] + }, + { + "cell_type": "code", + "execution_count": 25, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "false" + ] + }, + "execution_count": 25, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "TestIfSE3([1.0 0.0 0.0 1.2; 0.0 0.1 -0.95 1.5; 0.0 1.0 0.1 -0.9; 0.0 0.0 0.1 0.98])" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## CHAPTER 4: FORWARD KINEMATICS" + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "metadata": {}, + "outputs": [], + "source": [ + "M = [ -1 0 0 0 ;\n", + " 0 1 0 6 ;\n", + " 0 0 -1 2 ;\n", + " 0 0 0 1 ]\n", + "\n", + "Blist = [ 0 0 -1 2 0 0 ;\n", + " 0 0 0 0 1 0 ;\n", + " 0 0 1 0 0 0.1 ]'\n", + "\n", + "Slist = [0 0 1 4 0 0 ;\n", + " 0 0 0 0 1 0 ;\n", + " 0 0 -1 -6 0 -0.1 ]'\n", + "\n", + "thetalist = [π / 2.0, 3, π];" + ] + }, + { + "cell_type": "code", + "execution_count": 27, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4×4 Array{Float64,2}:\n", + " -1.14424e-17 1.0 0.0 -5.0 \n", + " 1.0 1.14424e-17 0.0 4.0 \n", + " 0.0 0.0 -1.0 1.68584\n", + " 0.0 0.0 0.0 1.0 " + ] + }, + "execution_count": 27, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "FKinBody(M, Blist, thetalist)" + ] + }, + { + "cell_type": "code", + "execution_count": 28, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4×4 Array{Float64,2}:\n", + " -1.14424e-17 1.0 0.0 -5.0 \n", + " 1.0 1.14424e-17 0.0 4.0 \n", + " 0.0 0.0 -1.0 1.68584\n", + " 0.0 0.0 0.0 1.0 " + ] + }, + "execution_count": 28, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "FKinSpace(M, Slist, thetalist)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## CHAPTER 5: VELOCITY KINEMATICS AND STATICS" + ] + }, + { + "cell_type": "code", + "execution_count": 29, + "metadata": {}, + "outputs": [], + "source": [ + "thetalist = [0.2, 1.1, 0.1, 1.2]\n", + "\n", + "Blist = [0 0 1 0 0.2 0.2 ;\n", + " 1 0 0 2 0 3 ;\n", + " 0 1 0 0 2 1 ;\n", + " 1 0 0 0.2 0.3 0.4 ]'\n", + "\n", + "Slist = [0 0 1 0 0.2 0.2 ;\n", + " 1 0 0 2 0 3 ;\n", + " 0 1 0 0 2 1 ;\n", + " 1 0 0 0.2 0.3 0.4 ]';" + ] + }, + { + "cell_type": "code", + "execution_count": 30, + "metadata": { + "scrolled": false + }, + "outputs": [ + { + "data": { + "text/plain": [ + "6×4 Array{Float64,2}:\n", + " -0.0452841 0.995004 0.0 1.0\n", + " 0.743593 0.0930486 0.362358 0.0\n", + " -0.667097 0.0361754 -0.932039 0.0\n", + " 2.32586 1.66809 0.564108 0.2\n", + " -1.44321 2.94561 1.43307 0.3\n", + " -2.0664 1.82882 -1.58869 0.4" + ] + }, + "execution_count": 30, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "JacobianBody(Blist, thetalist)" + ] + }, + { + "cell_type": "code", + "execution_count": 31, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "6×4 Array{Float64,2}:\n", + " 0.0 0.980067 -0.0901156 0.957494 \n", + " 0.0 0.198669 0.444554 0.284876 \n", + " 1.0 0.0 0.891207 -0.0452841\n", + " 0.0 1.95219 -2.21635 -0.511615 \n", + " 0.2 0.436541 -2.43713 2.77536 \n", + " 0.2 2.96027 3.23573 2.22512 " + ] + }, + "execution_count": 31, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "JacobianSpace(Slist, thetalist)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## CHAPTER 6: INVERSE KINEMATICS" + ] + }, + { + "cell_type": "code", + "execution_count": 32, + "metadata": {}, + "outputs": [], + "source": [ + "Blist = [ 0 0 -1 2 0 0 ;\n", + " 0 0 0 0 1 0 ;\n", + " 0 0 1 0 0 0.1 ]'\n", + "\n", + "Slist = [0 0 1 4 0 0 ;\n", + " 0 0 0 0 1 0 ;\n", + " 0 0 -1 -6 0 -0.1 ]'\n", + "\n", + "M = [ -1 0 0 0 ;\n", + " 0 1 0 6 ;\n", + " 0 0 -1 2 ;\n", + " 0 0 0 1 ]\n", + "\n", + "T = [ 0 1 0 -5 ;\n", + " 1 0 0 4 ;\n", + " 0 0 -1 1.6858 ;\n", + " 0 0 0 1 ]\n", + "\n", + "thetalist0 = [1.5, 2.5, 3]\n", + "\n", + "eomg = 0.01\n", + "\n", + "ev = 0.001;" + ] + }, + { + "cell_type": "code", + "execution_count": 33, + "metadata": { + "scrolled": false + }, + "outputs": [ + { + "data": { + "text/plain": [ + "([1.57074; 2.99967; 3.14154], true)" + ] + }, + "execution_count": 33, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "IKinBody(Blist, M, T, thetalist0, eomg, ev)" + ] + }, + { + "cell_type": "code", + "execution_count": 34, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "([1.57074; 2.99966; 3.14153], true)" + ] + }, + "execution_count": 34, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "IKinSpace(Slist, M, T, thetalist0, eomg, ev)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## CHAPTER 8: DYNAMICS OF OPEN CHAINS" + ] + }, + { + "cell_type": "code", + "execution_count": 35, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "6×6 Array{Float64,2}:\n", + " 0.0 -3.0 2.0 0.0 0.0 0.0\n", + " 3.0 0.0 -1.0 0.0 0.0 0.0\n", + " -2.0 1.0 0.0 0.0 0.0 0.0\n", + " 0.0 -6.0 5.0 0.0 -3.0 2.0\n", + " 6.0 0.0 -4.0 3.0 0.0 -1.0\n", + " -5.0 4.0 0.0 -2.0 1.0 0.0" + ] + }, + "execution_count": 35, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "ad([1, 2, 3, 4, 5, 6])" + ] + }, + { + "cell_type": "code", + "execution_count": 36, + "metadata": {}, + "outputs": [], + "source": [ + "thetalist = [0.1, 0.1, 0.1]\n", + "\n", + "dthetalist = [0.1, 0.2, 0.3]\n", + "\n", + "ddthetalist = [2, 1.5, 1]\n", + "\n", + "taulist = [0.5, 0.6, 0.7]\n", + "\n", + "g = [0, 0, -9.8]\n", + "\n", + "Ftip = [1, 1, 1, 1, 1, 1]\n", + "\n", + "M01 = [ 1 0 0 0 ;\n", + " 0 1 0 0 ;\n", + " 0 0 1 0.089159 ;\n", + " 0 0 0 1 ]\n", + "\n", + "M12 = [ 0 0 1 0.28 ;\n", + " 0 1 0 0.13585 ;\n", + " -1 0 0 0 ;\n", + " 0 0 0 1 ]\n", + "\n", + "M23 = [ 1 0 0 0 ;\n", + " 0 1 0 -0.1197 ;\n", + " 0 0 1 0.395 ;\n", + " 0 0 0 1 ]\n", + "\n", + "M34 = [ 1 0 0 0 ;\n", + " 0 1 0 0 ;\n", + " 0 0 1 0.14225 ;\n", + " 0 0 0 1 ]\n", + "\n", + "Mlist = [M01, M12, M23, M34]\n", + "\n", + "G1 = linalg.Diagonal([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7])\n", + "G2 = linalg.Diagonal([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393])\n", + "G3 = linalg.Diagonal([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275])\n", + "\n", + "Glist = [G1, G2, G3]\n", + "\n", + "Slist = [ 1 0 1 0 1 0 ;\n", + " 0 1 0 -0.089 0 0 ;\n", + " 0 1 0 -0.089 0 0.425 ]';" + ] + }, + { + "cell_type": "code", + "execution_count": 37, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3-element Array{Float64,1}:\n", + " 74.69616155287451 \n", + " -33.06766015851458 \n", + " -3.230573137901424" + ] + }, + "execution_count": 37, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, Glist, Slist)" + ] + }, + { + "cell_type": "code", + "execution_count": 38, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3×3 Array{Float64,2}:\n", + " 22.5433 -0.307147 -0.00718426\n", + " -0.307147 1.96851 0.432157 \n", + " -0.00718426 0.432157 0.191631 " + ] + }, + "execution_count": 38, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "MassMatrix(thetalist, Mlist, Glist, Slist)" + ] + }, + { + "cell_type": "code", + "execution_count": 39, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3-element Array{Float64,1}:\n", + " 0.26453118054501235 \n", + " -0.0550515682891655 \n", + " -0.006891320068248911" + ] + }, + "execution_count": 39, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist)" + ] + }, + { + "cell_type": "code", + "execution_count": 40, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3-element Array{Float64,1}:\n", + " 28.40331261821983 \n", + " -37.64094817177068 \n", + " -5.4415891999683605" + ] + }, + "execution_count": 40, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "GravityForces(thetalist, g, Mlist, Glist, Slist)" + ] + }, + { + "cell_type": "code", + "execution_count": 41, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3-element Array{Float64,1}:\n", + " 1.4095460782639782\n", + " 1.8577149723180628\n", + " 1.392409 " + ] + }, + "execution_count": 41, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist)" + ] + }, + { + "cell_type": "code", + "execution_count": 42, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3-element Array{Float64,1}:\n", + " -0.9739290670855626\n", + " 25.584667840340558 \n", + " -32.91499212478149 " + ] + }, + "execution_count": 42, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, Glist, Slist)" + ] + }, + { + "cell_type": "code", + "execution_count": 43, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "([0.11, 0.12, 0.13], [0.3, 0.35, 0.4])" + ] + }, + "execution_count": 43, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "EulerStep(thetalist, dthetalist, ddthetalist, 0.1)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Inverse Dynamics Trajectory\n", + "\n", + "Create a trajectory to follow using functions from Chapter 9." + ] + }, + { + "cell_type": "code", + "execution_count": 44, + "metadata": {}, + "outputs": [], + "source": [ + "thetastart = [0, 0, 0]\n", + "\n", + "thetaend = [π / 2, π / 2, π / 2]\n", + "\n", + "Tf = 3\n", + "\n", + "N = 1000\n", + "\n", + "method = 5\n", + "\n", + "traj = JointTrajectory(thetastart, thetaend, Tf, N, method)\n", + "\n", + "thetamat = copy(traj)\n", + "dthetamat = zeros(1000, 3)\n", + "ddthetamat = zeros(1000, 3)\n", + "\n", + "dt = Tf / (N - 1.0)\n", + "\n", + "for i = 1:size(traj, 1) - 1\n", + " dthetamat[i + 1, :] = (thetamat[i + 1, :] - thetamat[i, :]) / dt\n", + " ddthetamat[i + 1, :] = (dthetamat[i + 1, :] - dthetamat[i, :]) / dt\n", + "end" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Initialize robot description (Example with 3 links)" + ] + }, + { + "cell_type": "code", + "execution_count": 45, + "metadata": {}, + "outputs": [], + "source": [ + "g = [0, 0, -9.8]\n", + " \n", + "Ftipmat = ones(N, 6)\n", + " \n", + "M01 = [ 1 0 0 0 ;\n", + " 0 1 0 0 ;\n", + " 0 0 1 0.089159 ;\n", + " 0 0 0 1 ]\n", + "\n", + "M12 = [ 0 0 1 0.28 ;\n", + " 0 1 0 0.13585 ;\n", + " -1 0 0 0 ;\n", + " 0 0 0 1 ]\n", + "\n", + "M23 = [ 1 0 0 0 ;\n", + " 0 1 0 -0.1197 ;\n", + " 0 0 1 0.395 ;\n", + " 0 0 0 1 ]\n", + "\n", + "M34 = [ 1 0 0 0 ;\n", + " 0 1 0 0 ;\n", + " 0 0 1 0.14225 ;\n", + " 0 0 0 1 ]\n", + "\n", + "Mlist = [M01, M12, M23, M34]\n", + "\n", + "G1 = linalg.Diagonal([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7])\n", + "G2 = linalg.Diagonal([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393])\n", + "G3 = linalg.Diagonal([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275])\n", + "\n", + "Glist = [G1, G2, G3]\n", + "\n", + "Slist = [ 1 0 1 0 1 0 ;\n", + " 0 1 0 -0.089 0 0 ;\n", + " 0 1 0 -0.089 0 0.425 ]';" + ] + }, + { + "cell_type": "code", + "execution_count": 46, + "metadata": { + "scrolled": false + }, + "outputs": [], + "source": [ + "taumat = InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, Ftipmat, Mlist, Glist, Slist);" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Plot the joint forces/torques" + ] + }, + { + "cell_type": "code", + "execution_count": 47, + "metadata": {}, + "outputs": [], + "source": [ + "using Plots\n", + "gr();" + ] + }, + { + "cell_type": "code", + "execution_count": 48, + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "1.0\n", + "\n", + "\n", + "1.5\n", + "\n", + "\n", + "2.0\n", + "\n", + "\n", + "2.5\n", + "\n", + "\n", + "3.0\n", + "\n", + "\n", + "-25\n", + "\n", + "\n", + "0\n", + "\n", + "\n", + "25\n", + "\n", + "\n", + "50\n", + "\n", + "\n", + "75\n", + "\n", + "\n", + "100\n", + "\n", + "\n", + "Plot of Torque Trajectories\n", + "\n", + "\n", + "Time\n", + "\n", + "\n", + "Torque\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "Tau 1\n", + "\n", + "\n", + "\n", + "Tau 2\n", + "\n", + "\n", + "\n", + "Tau 3\n", + "\n", + "\n" + ] + }, + "execution_count": 48, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "timestamp = range(1, Tf, length=N)\n", + "\n", + "plot(timestamp, taumat[:, 1], linewidth=2, label=\"Tau 1\")\n", + "plot!(timestamp, taumat[:, 2], linewidth=2, label=\"Tau 2\")\n", + "plot!(timestamp, taumat[:, 3], linewidth=2, label=\"Tau 3\")\n", + "\n", + "xlabel!(\"Time\")\n", + "ylabel!(\"Torque\")\n", + "title!(\"Plot of Torque Trajectories\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Forward Dynamics Trajectory" + ] + }, + { + "cell_type": "code", + "execution_count": 49, + "metadata": {}, + "outputs": [], + "source": [ + "dt = 0.1\n", + "\n", + "intRes = 8\n", + "\n", + "thetalist = [0.1, 0.1, 0.1]\n", + "\n", + "dthetalist = [0.1, 0.2, 0.3]\n", + "\n", + "taumat = [[3.63, -6.58, -5.57], [3.74, -5.55, -5.5],\n", + " [4.31, -0.68, -5.19], [5.18, 5.63, -4.31],\n", + " [5.85, 8.17, -2.59], [5.78, 2.79, -1.7],\n", + " [4.99, -5.3, -1.19], [4.08, -9.41, 0.07],\n", + " [3.56, -10.1, 0.97], [3.49, -9.41, 1.23]]\n", + "\n", + "taumat = cat(taumat..., dims=2)';" + ] + }, + { + "cell_type": "code", + "execution_count": 50, + "metadata": {}, + "outputs": [], + "source": [ + "thetamat, dthetamat = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g,\n", + " Ftipmat, Mlist, Glist, Slist, dt, intRes);" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Plot the joint angle/velocities" + ] + }, + { + "cell_type": "code", + "execution_count": 51, + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "0.00\n", + "\n", + "\n", + "0.25\n", + "\n", + "\n", + "0.50\n", + "\n", + "\n", + "0.75\n", + "\n", + "\n", + "1.00\n", + "\n", + "\n", + "-10\n", + "\n", + "\n", + "-5\n", + "\n", + "\n", + "0\n", + "\n", + "\n", + "5\n", + "\n", + "\n", + "Plot of Joint Angles and Joint Velocities\n", + "\n", + "\n", + "Time\n", + "\n", + "\n", + "Joint Angles/Velocities\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "Theta1\n", + "\n", + "\n", + "\n", + "Theta2\n", + "\n", + "\n", + "\n", + "Theta3\n", + "\n", + "\n", + "\n", + "DTheta1\n", + "\n", + "\n", + "\n", + "DTheta2\n", + "\n", + "\n", + "\n", + "DTheta3\n", + "\n", + "\n" + ] + }, + "execution_count": 51, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "theta1 = thetamat[:, 1]\n", + "theta2 = thetamat[:, 2]\n", + "theta3 = thetamat[:, 3]\n", + "\n", + "dtheta1 = dthetamat[:, 1]\n", + "dtheta2 = dthetamat[:, 2]\n", + "dtheta3 = dthetamat[:, 3]\n", + "\n", + "N = size(taumat, 1)\n", + "Tf = size(taumat, 1) * dt\n", + "\n", + "timestamp = range(0, Tf, length=N)\n", + "\n", + "plot(timestamp, theta1, linewidth=2, label=\"Theta1\")\n", + "plot!(timestamp, theta2, linewidth=2, label=\"Theta2\")\n", + "plot!(timestamp, theta3, linewidth=2, label=\"Theta3\")\n", + "\n", + "plot!(timestamp, dtheta1, linewidth=2, label=\"DTheta1\")\n", + "plot!(timestamp, dtheta2, linewidth=2, label=\"DTheta2\")\n", + "plot!(timestamp, dtheta3, linewidth=2, label=\"DTheta3\")\n", + "\n", + "xlabel!(\"Time\")\n", + "ylabel!(\"Joint Angles/Velocities\")\n", + "title!(\"Plot of Joint Angles and Joint Velocities\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## CHAPTER 9: TRAJECTORY GENERATION" + ] + }, + { + "cell_type": "code", + "execution_count": 52, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "0.21600000000000003" + ] + }, + "execution_count": 52, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "CubicTimeScaling(2, 0.6)" + ] + }, + { + "cell_type": "code", + "execution_count": 53, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "0.16308" + ] + }, + "execution_count": 53, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "QuinticTimeScaling(2, 0.6)" + ] + }, + { + "cell_type": "code", + "execution_count": 54, + "metadata": { + "scrolled": true + }, + "outputs": [ + { + "data": { + "text/plain": [ + "6×8 LinearAlgebra.Adjoint{Float64,Array{Float64,2}}:\n", + " 1.0 0.0 0.0 1.0 1.0 0.2 0.0 1.0\n", + " 1.0208 0.052 0.0624 1.0104 1.104 0.3872 0.0936 1.0\n", + " 1.0704 0.176 0.2112 1.0352 1.352 0.8336 0.3168 1.0\n", + " 1.1296 0.324 0.3888 1.0648 1.648 1.3664 0.5832 1.0\n", + " 1.1792 0.448 0.5376 1.0896 1.896 1.8128 0.8064 1.0\n", + " 1.2 0.5 0.6 1.1 2.0 2.0 0.9 1.0" + ] + }, + "execution_count": 54, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "thetastart = [1, 0, 0, 1, 1, 0.2, 0,1]\n", + "thetaend = [1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1]\n", + "\n", + "Tf = 4\n", + "N = 6\n", + "method = 3\n", + "\n", + "JointTrajectory(thetastart, thetaend, Tf, N, method)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## CHAPTER 11: ROBOT CONTROL" + ] + }, + { + "cell_type": "code", + "execution_count": 55, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3-element Array{Float64,1}:\n", + " 133.0052524649953 \n", + " -29.942233243760633 \n", + " -3.0327685616172397" + ] + }, + "execution_count": 55, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "eint = [0.2, 0.2, 0.2]\n", + "\n", + "thetalistd = [1.0, 1.0, 1.0]\n", + "dthetalistd = [2, 1.2, 2]\n", + "ddthetalistd = [0.1, 0.1, 0.1]\n", + "\n", + "Kp = 1.3\n", + "Ki = 1.2\n", + "Kd = 1.1\n", + "\n", + "ComputedTorque(thetalist, dthetalist, eint, g, Mlist, Glist, Slist,\n", + " thetalistd, dthetalistd, ddthetalistd, Kp, Ki, Kd)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Simulate Control" + ] + }, + { + "cell_type": "code", + "execution_count": 56, + "metadata": {}, + "outputs": [], + "source": [ + "dt = 0.01\n", + "\n", + "# Create a trajectory to follow\n", + "\n", + "thetaend = [π / 2, π, 1.5 * π]\n", + "\n", + "Tf = 1\n", + "N = round(Int, Tf / dt)\n", + "method = 5\n", + "\n", + "traj = JointTrajectory(thetalist, thetaend, Tf, N, method)\n", + "\n", + "thetamatd = copy(traj)\n", + "dthetamatd = zeros(N, 3)\n", + "ddthetamatd = zeros(N, 3)\n", + "\n", + "dt = Tf / (N - 1)\n", + "\n", + "for i = 1:size(traj, 1)-1\n", + " dthetamatd[i + 1, :] = (thetamatd[i + 1, :] - thetamatd[i, :]) / dt\n", + " ddthetamatd[i + 1, :] = (dthetamatd[i + 1, :] - dthetamatd[i, :]) / dt\n", + "end\n", + "\n", + "# Possibly wrong robot description (Example with 3 links)\n", + "\n", + "gtilde = [0.8, 0.2, -8.8]\n", + "\n", + "Mhat01 = [1 0 0 0 ;\n", + " 0 1 0 0 ;\n", + " 0 0 1 0.1 ;\n", + " 0 0 0 1 ]\n", + "\n", + "Mhat12 = [ 0 0 1 0.3 ;\n", + " 0 1 0 0.2 ;\n", + " -1 0 0 0 ;\n", + " 0 0 0 1 ]\n", + "\n", + "Mhat23 = [1 0 0 0 ;\n", + " 0 1 0 -0.2 ;\n", + " 0 0 1 0.4 ;\n", + " 0 0 0 1 ]\n", + "\n", + "Mhat34 = [1 0 0 0 ;\n", + " 0 1 0 0 ;\n", + " 0 0 1 0.2 ;\n", + " 0 0 0 1 ]\n", + "\n", + "Mtildelist = [Mhat01, Mhat12, Mhat23, Mhat34]\n", + "\n", + "Ghat1 = linalg.Diagonal([0.1, 0.1, 0.1, 4, 4, 4])\n", + "Ghat2 = linalg.Diagonal([0.3, 0.3, 0.1, 9, 9, 9])\n", + "Ghat3 = linalg.Diagonal([0.1, 0.1, 0.1, 3, 3, 3])\n", + "\n", + "Gtildelist = [Ghat1, Ghat2, Ghat3]\n", + "\n", + "Ftipmat = ones(size(traj, 1), 6)\n", + "\n", + "Kp = 20\n", + "Ki = 10\n", + "Kd = 18\n", + "\n", + "intRes = 8;" + ] + }, + { + "cell_type": "code", + "execution_count": 57, + "metadata": {}, + "outputs": [], + "source": [ + "taumat, thetamat = SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist,\n", + " Slist, thetamatd, dthetamatd, ddthetamatd, gtilde,\n", + " Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes);" + ] + }, + { + "cell_type": "code", + "execution_count": 58, + "metadata": { + "scrolled": false + }, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "0.00\n", + "\n", + "\n", + "0.25\n", + "\n", + "\n", + "0.50\n", + "\n", + "\n", + "0.75\n", + "\n", + "\n", + "1.00\n", + "\n", + "\n", + "0\n", + "\n", + "\n", + "1\n", + "\n", + "\n", + "2\n", + "\n", + "\n", + "3\n", + "\n", + "\n", + "4\n", + "\n", + "\n", + "Plot of Actual and Desired Joint Angles\n", + "\n", + "\n", + "Time\n", + "\n", + "\n", + "Joint Angles\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "ActualTheta 1\n", + "\n", + "\n", + "\n", + "DesiredTheta 1\n", + "\n", + "\n", + "\n", + "ActualTheta 2\n", + "\n", + "\n", + "\n", + "DesiredTheta 2\n", + "\n", + "\n", + "\n", + "ActualTheta 3\n", + "\n", + "\n", + "\n", + "DesiredTheta 3\n", + "\n", + "\n" + ] + }, + "execution_count": 58, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "N, links = size(thetamat)\n", + "\n", + "Tf = N * dt\n", + "\n", + "timestamp = range(0, Tf, length=N)\n", + "\n", + "plot()\n", + "\n", + "for i = 1:links\n", + " plot!(timestamp, thetamat[:, i], lw=2, linestyle=:dash, label=\"ActualTheta $i\")\n", + " plot!(timestamp, thetamatd[:, i], lw=2, linestyle=:dot, label=\"DesiredTheta $i\")\n", + "end\n", + "\n", + "xlabel!(\"Time\")\n", + "ylabel!(\"Joint Angles\")\n", + "title!(\"Plot of Actual and Desired Joint Angles\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Julia 1.1.0", + "language": "julia", + "name": "julia-1.1" + }, + "language_info": { + "file_extension": ".jl", + "mimetype": "application/julia", + "name": "julia", + "version": "1.1.0" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}