From 123966ea283f2dd3aaff51371c0dce67816821ea Mon Sep 17 00:00:00 2001 From: Ioana Anamaria <ioana.ulici@student.utcluj.ro> Date: Tue, 11 Oct 2022 11:03:25 +0300 Subject: [PATCH 1/3] Small typos, updated real robot code, adding the al5d dynamic model from the toolbox --- al5d_control.ipynb | 123 +++++++++++++++++++++++++++++++ ald5_control.ipynb | 89 ---------------------- lab01_CoordinateSystems.ipynb | 100 ++++++++++++------------- lab02_DirectGeometricModel.ipynb | 12 +-- lab03_DHconvention.ipynb | 36 +++++---- lab04_Jacobian.ipynb | 20 ++--- lab05_IKM.ipynb | 9 +-- lab06_Trajectories.ipynb | 25 ++++--- lab07_DynamicModel.ipynb | 78 +++++++++++++------- lab10_ControlDesign.ipynb | 17 +---- lab11_ControlDesign-Drones.ipynb | 19 +---- lab_functions.py | 9 ++- 12 files changed, 289 insertions(+), 248 deletions(-) create mode 100644 al5d_control.ipynb delete mode 100644 ald5_control.ipynb diff --git a/al5d_control.ipynb b/al5d_control.ipynb new file mode 100644 index 0000000..0c3f584 --- /dev/null +++ b/al5d_control.ipynb @@ -0,0 +1,123 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Robotic manipulator\n", + "\n", + "As you might have noticed, each table is equiped with a robotic arm. We will use this robotic arm for practical examples and exercises during the lab. This robot is your friend, so please treat it with love and care. It is not a problem if you break something, but it is a problem if you don't care. So please care :)\n", + "\n", + "This robot is a [Lynxmotion AL5D](http://www.lynxmotion.com/c-130-al5d.aspx). It has five degrees of freedom and a gripper. It is controlled by five servomotors of different sizes and power, connected on a [SSC-32U USB controller](http://www.lynxmotion.com/p-1032-ssc-32u-usb-servo-controller.aspx).\n", + "\n", + "For this first lab, we want to make sure that the robot is connected properly and is working. You need to execute the code below, which will initialise the robot in a specific position and then control each joint using sliders. You can inspect the code with comments in order to understand it better. Go ahead and give it a try!" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Importing necessary modules\n", + "import serial\n", + "import sys\n", + "import time\n", + "from ipywidgets import interact, interactive, fixed, interact_manual\n", + "import roboticstoolbox as rtb\n", + "import numpy as np\n", + "\n", + "al5d = rtb.models.URDF.AL5D_mdw()\n", + "\n", + "def robot_control(q0=0, q1=0, q2=0, q3=0, q4=0, gripper=0): #Method for controlling the joints\n", + " q0r = q0*np.pi/180\n", + " q1r = q1*np.pi/180\n", + " q2r = q2*np.pi/180\n", + " q3r = q3*np.pi/180\n", + " q4r = q4*np.pi/180\n", + " elbow = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow')\n", + " wrist = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist')\n", + " grip = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')\n", + " tool = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')\n", + " \n", + " if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):\n", + " print(\"Robot collision with the table\")\n", + " else:\n", + " print(\"No collision detected\")\n", + " pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", + " pwq1 = str(round((q1+90)*11.1111 + 500))\n", + " pwq2 = str(round((q2+90)*11.1111 + 500))\n", + " pwq3 = str(round((q3+90)*11.1111 + 500))\n", + " pwq4 = str(round((q4+90)*11.1111 + 500))\n", + " pwq5 = str(round((gripper+90)*11.1111 + 500))\n", + " # Concatinating the desired control signals in a long string\n", + " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\" + \"#4P\" + pwq4 + \"S1500\"+ \"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", + " output = output.encode('utf-8') # Converting to bytes literals\n", + " ssc32.write(output) # sending serial data to the SSC-32 board\n", + "\n", + "devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9']\n", + " \n", + "# Starting the connection with the SSC-32U controller\n", + "for device in devices:\n", + " try:\n", + " ssc32 = serial.Serial(device, 9600, timeout=1.0)\n", + " except serial.SerialException:\n", + " print(\"No device connected on \", device)\n", + "# Initializing the position of the robot\n", + "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", + "# Initialising the sliders\n", + "interact(robot_control, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90))" + ] + } + ], + "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.8.8" + }, + "varInspector": { + "cols": { + "lenName": 16, + "lenType": 16, + "lenVar": 40 + }, + "kernels_config": { + "python": { + "delete_cmd_postfix": "", + "delete_cmd_prefix": "del ", + "library": "var_list.py", + "varRefreshCmd": "print(var_dic_list())" + }, + "r": { + "delete_cmd_postfix": ") ", + "delete_cmd_prefix": "rm(", + "library": "var_list.r", + "varRefreshCmd": "cat(var_dic_list()) " + } + }, + "types_to_exclude": [ + "module", + "function", + "builtin_function_or_method", + "instance", + "_Feature" + ], + "window_display": false + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/ald5_control.ipynb b/ald5_control.ipynb deleted file mode 100644 index 347ccb7..0000000 --- a/ald5_control.ipynb +++ /dev/null @@ -1,89 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Robotic manipulator\n", - "\n", - "As you might have noticed, each table is equiped with a robotic arm. We will use this robotic arm for practical examples and exercises during the lab. This robot is your friend, so please treat it with love and care. It is not a problem if you break something, but it is a problem if you don't care. So please care :)\n", - "\n", - "This robot is a [Lynxmotion AL5D](http://www.lynxmotion.com/c-130-al5d.aspx). It has four degrees of freedom and a gripper. It is controlled by five servomotors of different sizes and power, connected on a [SSC-32U USB controller](http://www.lynxmotion.com/p-1032-ssc-32u-usb-servo-controller.aspx).\n", - "\n", - "For this first lab, we want to make sure that the robot is connected properly and is working. You need to execute the code below, which will initialise the robot in a specific position and then control each joint using sliders. You can inspect the code with comments in order to understand it better. Go ahead and give it a try!" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": {}, - "outputs": [ - { - "ename": "SerialException", - "evalue": "[Errno 2] could not open port ttyUSB0: [Errno 2] No such file or directory: 'ttyUSB0'", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mFileNotFoundError\u001b[0m Traceback (most recent call last)", - "\u001b[0;32m/usr/lib/python3.9/site-packages/serial/serialposix.py\u001b[0m in \u001b[0;36mopen\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 321\u001b[0m \u001b[0;32mtry\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 322\u001b[0;31m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mfd\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mopen\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mportstr\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mO_RDWR\u001b[0m \u001b[0;34m|\u001b[0m \u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mO_NOCTTY\u001b[0m \u001b[0;34m|\u001b[0m \u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mO_NONBLOCK\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 323\u001b[0m \u001b[0;32mexcept\u001b[0m \u001b[0mOSError\u001b[0m \u001b[0;32mas\u001b[0m \u001b[0mmsg\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", - "\u001b[0;31mFileNotFoundError\u001b[0m: [Errno 2] No such file or directory: 'ttyUSB0'", - "\nDuring handling of the above exception, another exception occurred:\n", - "\u001b[0;31mSerialException\u001b[0m Traceback (most recent call last)", - "\u001b[0;32m/tmp/ipykernel_248207/3815026025.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m 19\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 20\u001b[0m \u001b[0;31m# Starting the connection with the SSC-32U controller\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 21\u001b[0;31m \u001b[0mssc32\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mserial\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mSerial\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'ttyUSB0'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m9600\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtimeout\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m1.0\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 22\u001b[0m \u001b[0;31m# Initializing the position of the robot\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 23\u001b[0m \u001b[0mssc32\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mwrite\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34mb'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r'\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", - "\u001b[0;32m/usr/lib/python3.9/site-packages/serial/serialutil.py\u001b[0m in \u001b[0;36m__init__\u001b[0;34m(self, port, baudrate, bytesize, parity, stopbits, timeout, xonxoff, rtscts, write_timeout, dsrdtr, inter_byte_timeout, exclusive, **kwargs)\u001b[0m\n\u001b[1;32m 242\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 243\u001b[0m \u001b[0;32mif\u001b[0m \u001b[0mport\u001b[0m \u001b[0;32mis\u001b[0m \u001b[0;32mnot\u001b[0m \u001b[0;32mNone\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 244\u001b[0;31m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mopen\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 245\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 246\u001b[0m \u001b[0;31m# - - - - - - - - - - - - - - - - - - - - - - - -\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", - "\u001b[0;32m/usr/lib/python3.9/site-packages/serial/serialposix.py\u001b[0m in \u001b[0;36mopen\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 323\u001b[0m \u001b[0;32mexcept\u001b[0m \u001b[0mOSError\u001b[0m \u001b[0;32mas\u001b[0m \u001b[0mmsg\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 324\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mfd\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;32mNone\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 325\u001b[0;31m \u001b[0;32mraise\u001b[0m \u001b[0mSerialException\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mmsg\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0merrno\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m\"could not open port {}: {}\"\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mformat\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_port\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmsg\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 326\u001b[0m \u001b[0;31m#~ fcntl.fcntl(self.fd, fcntl.F_SETFL, 0) # set blocking\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 327\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n", - "\u001b[0;31mSerialException\u001b[0m: [Errno 2] could not open port ttyUSB0: [Errno 2] No such file or directory: 'ttyUSB0'" - ] - } - ], - "source": [ - "# Importing necessary modules\n", - "import serial\n", - "import sys\n", - "import time\n", - "from __future__ import print_function\n", - "from ipywidgets import interact, interactive, fixed, interact_manual\n", - "import ipywidgets as widgets\n", - "\n", - "def fkine(q0,q1,q2,q3,q4): #Method for controlling the joints\n", - " pwq0 = str(round(q0*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", - " pwq1 = str(round(q1*11.1111 + 500))\n", - " pwq2 = str(round(q2*11.1111 + 500))\n", - " pwq3 = str(round(q3*11.1111 + 500))\n", - " pwq4 = str(round(q4*11.1111 + 500))\n", - " # Concatinating the desired control signals in a long string\n", - " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\"+\"#4P\" + pwq4 + \"S1500\"+\"\\r\" # Formatting string\n", - " output = output.encode('utf-8') # Converting to bytes literals\n", - " ssc32.write(output) # sending serial data to the SSC-32 board\n", - "\n", - "# Starting the connection with the SSC-32U controller\n", - "ssc32 = serial.Serial('ttyUSB0', 9600, timeout=1.0)\n", - "# Initializing the position of the robot\n", - "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", - "# Initialising the sliders\n", - "interact(fkine, q0=(0,180),q1=(0,180),q2=(0,180),q3=(0,180),q4=(0,180))" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "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.9.6" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/lab01_CoordinateSystems.ipynb b/lab01_CoordinateSystems.ipynb index dbb9db1..be9a6dd 100644 --- a/lab01_CoordinateSystems.ipynb +++ b/lab01_CoordinateSystems.ipynb @@ -147,7 +147,7 @@ "source": [ "from ipywidgets import interact\n", "from spatialmath.base import *\n", - "from lab01_functions import setPlot, plotAxes\n", + "from lab_functions import setPlot, plotAxes\n", "\n", "def TransX(d):\n", " ax = setPlot()\n", @@ -177,7 +177,7 @@ "source": [ "from ipywidgets import interact\n", "from spatialmath.base import *\n", - "from lab01_functions import setPlot, plotAxes\n", + "from lab_functions import setPlot, plotAxes\n", "\n", "def TransY(d):\n", " ax = setPlot()\n", @@ -207,7 +207,7 @@ "source": [ "from ipywidgets import interact\n", "from spatialmath.base import *\n", - "from lab01_functions import setPlot, plotAxes\n", + "from lab_functions import setPlot, plotAxes\n", "\n", "def TransZ(d):\n", " ax = setPlot()\n", @@ -237,7 +237,7 @@ "source": [ "from ipywidgets import interact\n", "from spatialmath.base import *\n", - "from lab01_functions import setPlot, plotAxes\n", + "from lab_functions import setPlot, plotAxes\n", "\n", "def RotX(theta):\n", " ax = setPlot()\n", @@ -267,7 +267,7 @@ "source": [ "from ipywidgets import interact\n", "from spatialmath.base import *\n", - "from lab01_functions import setPlot, plotAxes\n", + "from lab_functions import setPlot, plotAxes\n", "\n", "def RotY(theta):\n", " ax = setPlot()\n", @@ -297,7 +297,7 @@ "source": [ "from ipywidgets import interact\n", "from spatialmath.base import *\n", - "from lab01_functions import setPlot, plotAxes\n", + "from lab_functions import setPlot, plotAxes\n", "\n", "def RotZ(theta):\n", " ax = setPlot()\n", @@ -645,7 +645,7 @@ "outputs": [], "source": [ "from ipywidgets import interact, widgets\n", - "from lab01_functions import setPlot, plotAxes\n", + "from lab_functions import setPlot, plotAxes\n", "from spatialmath.base import *\n", "import numpy as np\n", "\n", @@ -696,7 +696,7 @@ "outputs": [], "source": [ "from ipywidgets import interact\n", - "from lab01_functions import setPlot, plotAxes\n", + "from lab_functions import setPlot, plotAxes\n", "from spatialmath.base import *\n", "import numpy as np\n", "\n", @@ -789,27 +789,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "For any library or methods used, you have the option to look at the Python documentation by using the [.__ doc __](https://blog.finxter.com/what-is-__-doc-__-in-python/) command." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "scrolled": false - }, - "outputs": [], - "source": [ - "import numpy as np\n", - "\n", - "print(np.__doc__)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "There are several toolboxes available online for helping us with various robotics operations. Ones we will use extensively during the laboratories is [__spatialmath__](https://petercorke.github.io/spatialmath-python/intro.html). This toolbox offers a very easy way to calculate various transformation matrices, using the __trotx, troty, trotz, and transl__ commands. You can see an example of how are these used below:" + "There are several toolboxes available online for helping us with various robotics operations. One we will use extensively during the laboratories is [__spatialmath__](https://petercorke.github.io/spatialmath-python/intro.html). This toolbox offers a very easy way to calculate various transformation matrices, using the __trotx, troty, trotz, and transl__ commands. You can see an example of how are these used below:" ] }, { @@ -890,7 +870,7 @@ "In robotics, coordinate frames can be attached to different parts of a robot.\n", "Consider the model of the AL5D robot, shown below. \n", "- Identify the homogeneous transformation matrix that can express the frame attached to the gripper of the robot in the base frame. The origin of the gripper frame with respect to the base frame lies at $(0, 3, 0.5)$\n", - "- Connect the robot to your computer and run the code below. Try to move your robot to the pose shown in the figure using the sliders. What are the values of each joint $(q_0, q_1, q_2, q_3)$ in this the pose?\n", + "- Connect the robot to your computer and run the code below. Try to move your robot to the pose shown in the figure using the sliders. What are the values of each joint $(q_0, q_1, q_2, q_3, q_4)$ in this the pose?\n", "- A camera attached to the gripper of the robot would see the purple ball at the position $(-1, -0.5, 2)$ expressed in the gripper's frame. Find the coordinates of the ball expressed in the base frame.\n", "\n", "<center>\n", @@ -905,39 +885,59 @@ "cell_type": "code", "execution_count": null, "metadata": { - "code_folding": [ - 10 - ] + "code_folding": [] }, "outputs": [], "source": [ "### Cell for sending commands to the AL5D robot ###\n", - "\n", "# Importing necessary modules\n", - "from __future__ import print_function\n", "import serial\n", "import sys\n", "import time\n", "from ipywidgets import interact, interactive, fixed, interact_manual\n", - "import ipywidgets as widgets\n", - "\n", - "def fkine(q0,q1,q2,q3,q4): #Method for controlling the 4 joints and the gripper\n", - " pwq0 = str(round(q0*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", - " pwq1 = str(round(q1*11.1111 + 500))\n", - " pwq2 = str(round(q2*11.1111 + 500))\n", - " pwq3 = str(round(q3*11.1111 + 500))\n", - " pwq4 = str(round(q4*11.1111 + 500))\n", - " # Concatinating the desired control signals in a long string\n", - " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\"+\"#4P\" + pwq4 + \"S1500\"+\"\\r\" # Formatting string\n", - " output = output.encode('utf-8') # Converting to bytes literals\n", - " ssc32.write(output) # sending serial data to the SSC-32 board\n", + "import roboticstoolbox as rtb\n", + "import numpy as np\n", "\n", + "al5d = rtb.models.URDF.AL5D_mdw()\n", + "\n", + "def robot_control(q0=0, q1=0, q2=0, q3=0, q4=0, gripper=0): #Method for controlling the joints\n", + " q0r = q0*np.pi/180\n", + " q1r = q1*np.pi/180\n", + " q2r = q2*np.pi/180\n", + " q3r = q3*np.pi/180\n", + " q4r = q4*np.pi/180\n", + " elbow = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow')\n", + " wrist = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist')\n", + " grip = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')\n", + " tool = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')\n", + " \n", + " if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):\n", + " print(\"Robot collision with the table\")\n", + " else:\n", + " print(\"No collision detected\")\n", + " pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", + " pwq1 = str(round((q1+90)*11.1111 + 500))\n", + " pwq2 = str(round((q2+90)*11.1111 + 500))\n", + " pwq3 = str(round((q3+90)*11.1111 + 500))\n", + " pwq4 = str(round((q4+90)*11.1111 + 500))\n", + " pwq5 = str(round((gripper+90)*11.1111 + 500))\n", + " # Concatinating the desired control signals in a long string\n", + " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\" + \"#4P\" + pwq4 + \"S1500\"+ \"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", + " output = output.encode('utf-8') # Converting to bytes literals\n", + " ssc32.write(output) # sending serial data to the SSC-32 board\n", + "\n", + "devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9']\n", + " \n", "# Starting the connection with the SSC-32U controller\n", - "ssc32 = serial.Serial('COM3', 9600, timeout=1.0)\n", + "for device in devices:\n", + " try:\n", + " ssc32 = serial.Serial(device, 9600, timeout=1.0)\n", + " except serial.SerialException:\n", + " print(\"No device connected on \", device)\n", "# Initializing the position of the robot\n", "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", "# Initialising the sliders\n", - "interact(fkine, q0=(0,180),q1=(0,180),q2=(0,180),q3=(0,180),q4=(0,180))" + "interact(robot_control, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90))" ] }, { @@ -964,7 +964,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.7.6" + "version": "3.8.8" }, "varInspector": { "cols": { diff --git a/lab02_DirectGeometricModel.ipynb b/lab02_DirectGeometricModel.ipynb index 3b15b7e..773d1f2 100644 --- a/lab02_DirectGeometricModel.ipynb +++ b/lab02_DirectGeometricModel.ipynb @@ -527,14 +527,16 @@ "from ipywidgets import interact, interactive, fixed, interact_manual\n", "import ipywidgets as widgets\n", "\n", - "def fkine(q0,q1,q2,q3,q4): #Method for controlling the 4 joints and gripper\n", + "def fkine(q0,q1,q2,q3,q4, q5): #Method for controlling the joints\n", " pwq0 = str(round(q0*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", " pwq1 = str(round(q1*11.1111 + 500))\n", " pwq2 = str(round(q2*11.1111 + 500))\n", " pwq3 = str(round(q3*11.1111 + 500))\n", " pwq4 = str(round(q4*11.1111 + 500))\n", - " # Concatinating the desired control signals in a long string\n", - " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\"+\"#4P\" + pwq4 + \"S1500\"+\"\\r\" # Formatting string\n", + " pwq5 = str(round(q5*11.1111 + 500))\n", + "\n", + " # Concatenating the desired control signals in a long string\n", + " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\"+\"#4P\" + pwq4 + \"S1500\"+\"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", " output = output.encode('utf-8') # Converting to bytes literals\n", " ssc32.write(output) # sending serial data to the SSC-32 board\n", "\n", @@ -543,7 +545,7 @@ "# Initializing the position of the robot\n", "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", "# Initialising the sliders\n", - "interact(fkine, q0=(0,180),q1=(0,180),q2=(0,180),q3=(0,180),q4=(0,180))" + "interact(fkine, q0=(0,180),q1=(0,180),q2=(0,180),q3=(0,180),q4=(0,180), q5=(0,180))" ] }, { @@ -597,7 +599,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.7.6" + "version": "3.8.8" }, "varInspector": { "cols": { diff --git a/lab03_DHconvention.ipynb b/lab03_DHconvention.ipynb index a29ea2d..f4541ea 100644 --- a/lab03_DHconvention.ipynb +++ b/lab03_DHconvention.ipynb @@ -475,7 +475,7 @@ "metadata": {}, "outputs": [], "source": [ - "%matplotlib widget\n", + "#%matplotlib nbAgg\n", "from roboticstoolbox import *\n", "import numpy as np\n", "from math import *\n", @@ -483,8 +483,13 @@ "\n", "# a link may be Revolute or Prismatic\n", "\n", - "# DHLink object creates a link with a joint attached to it, arguments:\n", - "# theta, d, a(r) , alpha, offset, sigma(0 for revolute, 1 for prismatic), \n", + "# DHLink object creates a link with a joint attached to it, with arguments:\n", + "# theta, \n", + "# d, \n", + "# a(r), \n", + "# alpha, \n", + "# offset, \n", + "# sigma(0 for revolute, 1 for prismatic), \n", "# mdh (argument for choosing if the DH is standard or modified convention, the latter being the one we use, 1 for true) \n", "\n", "# or directly use one of: RevoluteDH, RevoluteMDH, PrismaticDH, PrismaticMDH which correspond to \n", @@ -493,7 +498,7 @@ "# you can use only the non-zero arguments when creating the Links\n", "\n", "Link1 = DHLink(alpha = 0, d = 0.3, a = 0, sigma = 0, mdh = 1, offset = pi/2);\n", - "Link2 = RevoluteMDH(alpha = pi/2)\n", + "Link2 = RevoluteMDH(alpha = pi/2) # this will default d,a,offset to 0\n", "Link3 = PrismaticMDH(alpha = pi/2)\n", "\n", "# we combine the links using DHRobot\n", @@ -501,14 +506,14 @@ " Link1,\n", " Link2,\n", " Link3,\n", - " RevoluteMDH(d=2)], name = \"my_robot\")\n", + " RevoluteMDH(d=2)], name = \"my_4links_robot\")\n", "\n", "# adding a tool that is -0.4 units away from the last joint, on x:\n", "rob_tool = DHRobot([\n", " Link1,\n", " Link2,\n", " Link3,\n", - " RevoluteMDH(d=2)], name = \"my_robot_with_tool\", tool = transl(-0.4, 0, 0)@trotz(pi) )\n", + " RevoluteMDH(d=2)], name = \"my_4links_robot_with_tool\", tool = transl(-0.4, 0, 0)@trotz(pi) )\n", "\n", "print(Link1)\n", "print(\"Tool transform\\n\",rob_tool.tool)\n", @@ -516,7 +521,7 @@ "# see the DH table of the robot\n", "print(rob)\n", "\n", - "# find the pose of the robot with the following joint coordinates:\n", + "# find the pose (forward kinematics) of the robot with the following joint coordinates:\n", "q = np.array([0, pi/2, 0.4, -pi/6])\n", "T = rob.fkine(q)\n", "print(\"Pose of rob at q=\" , q , \"\\n\",T)\n", @@ -614,9 +619,7 @@ "cell_type": "code", "execution_count": null, "metadata": { - "code_folding": [ - 10 - ] + "code_folding": [] }, "outputs": [], "source": [ @@ -630,14 +633,17 @@ "from ipywidgets import interact, interactive, fixed, interact_manual\n", "import ipywidgets as widgets\n", "\n", - "def fkine(q0,q1,q2,q3,q4): #Method for controlling the 4 joints and gripper\n", + "\n", + "def fkine(q0,q1,q2,q3,q4, q5): #Method for controlling the joints\n", " pwq0 = str(round(q0*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", " pwq1 = str(round(q1*11.1111 + 500))\n", " pwq2 = str(round(q2*11.1111 + 500))\n", " pwq3 = str(round(q3*11.1111 + 500))\n", " pwq4 = str(round(q4*11.1111 + 500))\n", - " # Concatinating the desired control signals in a long string\n", - " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\"+\"#4P\" + pwq4 + \"S1500\"+\"\\r\" # Formatting string\n", + " pwq5 = str(round(q5*11.1111 + 500))\n", + "\n", + " # Concatenating the desired control signals in a long string\n", + " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\"+\"#4P\" + pwq4 + \"S1500\"+\"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", " output = output.encode('utf-8') # Converting to bytes literals\n", " ssc32.write(output) # sending serial data to the SSC-32 board\n", "\n", @@ -646,7 +652,7 @@ "# Initializing the position of the robot\n", "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", "# Initialising the sliders\n", - "interact(fkine, q0=(0,180),q1=(0,180),q2=(0,180),q3=(0,180),q4=(0,180))" + "interact(fkine, q0=(0,180),q1=(0,180),q2=(0,180),q3=(0,180),q4=(0,180), q5=(0,180))" ] } ], @@ -666,7 +672,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.7.6" + "version": "3.8.8" }, "varInspector": { "cols": { diff --git a/lab04_Jacobian.ipynb b/lab04_Jacobian.ipynb index 868fbf6..1883c15 100644 --- a/lab04_Jacobian.ipynb +++ b/lab04_Jacobian.ipynb @@ -94,7 +94,7 @@ "In the videos below you can see how a 2D robot would move to get to a specific rotation of the end-effector (considered as the end of the red link). \n", "\n", "\n", - "|The movement of the whole robot|The pose of the end-effector |\n", + "|The movement of the whole robot|The movement of the end-effector |\n", "|--|--|\n", "|<video controls src=\"artwork/jacobian/angular_velocity_02.mp4\" width=400 />|<video controls src=\"artwork/jacobian/angular_velocity_01.mp4\" width=400 />|\n", "\n", @@ -167,7 +167,7 @@ "\n", "In the videos below you can see how a 2D robot would move to get to a specific translation of the end-effector (considered as the end of the green link). \n", "\n", - "|The movement of the whole robot|The pose of the end-effector |\n", + "|The movement of the whole robot|The movement of the end-effector |\n", "|--|--|\n", "|<video controls src=\"artwork/jacobian/linear_velocity_01.mp4\" width=400 />|<video controls src=\"artwork/jacobian/linear_velocity_02.mp4\" width=400 />|\n", "\n", @@ -236,9 +236,7 @@ { "cell_type": "code", "execution_count": null, - "metadata": { - "scrolled": true - }, + "metadata": {}, "outputs": [], "source": [ "%matplotlib widget\n", @@ -253,7 +251,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "For the same 2DOF robot as in the previous example, we shall see the variation of the velocities, by changing the q1 and q2 angles. The black ellipse represents the vectors of the possible end-effector velocities for a specific configuration of the robot. " + "For the same 2DOF robot as in the previous example, we shall see the variation of the velocities, by changing the q1 and q2 angles. The ellipse represents the vectors of the possible end-effector velocities for a specific configuration of the robot. " ] }, { @@ -393,9 +391,7 @@ { "cell_type": "code", "execution_count": null, - "metadata": { - "scrolled": true - }, + "metadata": {}, "outputs": [], "source": [ "import roboticstoolbox as rtb\n", @@ -419,9 +415,7 @@ { "cell_type": "code", "execution_count": null, - "metadata": { - "scrolled": false - }, + "metadata": {}, "outputs": [], "source": [ "import roboticstoolbox as rtb\n", @@ -651,7 +645,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.7.6" + "version": "3.8.8" }, "varInspector": { "cols": { diff --git a/lab05_IKM.ipynb b/lab05_IKM.ipynb index 39d5d91..10fc9a4 100644 --- a/lab05_IKM.ipynb +++ b/lab05_IKM.ipynb @@ -236,15 +236,14 @@ "In the previous laboratory, we saw how to define links and how to combine them\n", "into a robotic structure, using the __Revolute__, __Prismatic__ and __DHRobot__ methods. We also saw how to calculate the end effector pose using the __fkine__, the jacobian of the robot using the __jacob0__, and how to visualise the robot using the __plot__ methods.\n", "\n", - "In the examples above, it is 'easy' to calculate the inverse kinematics models by hand, but for more complex robots, we need to solve it numerically. The robopy toolbox has methods for solving the inverse kinematics model, using numerical methods.\n", - "method\n", + "In the examples above, it is 'easy' to calculate the inverse kinematics models by hand, but for more complex robots, we need to solve it numerically. The robotic toolbox has methods for solving the inverse kinematics model, using numerical methods.\n", "The toolbox can do this using the __ikine__ method (from inverse kinematics). The method works by providing a desired end-effector pose (position and orientation) and gives back the joint coordinates the result in the desired pose. As we know, the inverse kinematics model might have more than just one solution for a specific pose. The way numerical methods work, they start looking for a solution around a initial guess (which we can provide), and slowly converge to the joint coordinates that result in the desired pose. The convergence point might depend on the initial guess we are providing.\n", "\n", "## Usage\n", "\n", "The __ikine_LM__ method provides a vector of joint coordinates that results in the end-effector pose to be the one desired. To do so, we need to provide a pose as an input to the method. The pose is provided in the form of a 4x4 homogeneous transformation matrix. This transformation matrix provides information about the 6 degrees of freedom available (3 positions and 3 orientations).\n", "\n", - "The __ikine_LM__ method can solve the inverse kinematics problem even for kinematic chains with less than 6 degrees of freedom. In that case, we need to specify for which DoFs we want the inverse kinematics problem solved. We do this by providing a __mask__ as an input, which is a vector with six logical elements (0s or 1s), specifying with 1 those DoFs that we want to solve the inverse kinematics for. The number of DoFs that we can solve the inverse kinematics for must be equal to the DoFs of the robot itself.\n", + "The __ikine_LM__ method can solve the inverse kinematics problem even for kinematic chains with less than 6 degrees of freedom. In that case, we need to specify for which DoFs we want the inverse kinematics problem solved. We do this by providing a __mask__ as an input, which is a vector with six logical elements (0 or 1), specifying with 1 those DoFs that we want to solve the inverse kinematics for. The number of DoFs that we can solve the inverse kinematics for __must__ be equal to the DoFs of the robot itself.\n", "\n", "## Example\n", "\n", @@ -256,7 +255,7 @@ "* We verify that the inverse kinematics give the same solution as the input to the forward kinematics\n", "* We can also try to solve the inverse kinematics for any input pose (example 5.2.2)\n", "\n", - "For the input pose to be valid, we need to use the SE3 constructors from the spatial math module of the robotics toolbox. The corresponding methods are:\n", + "For the input pose to be valid, we need to use the __SE3__ constructors from the spatial math module of the robotics toolbox. The corresponding methods are:\n", "\n", "* Translation: SE3(x,y,z)\n", "* Rotation on X: SE3.Rx(theta)\n", @@ -426,7 +425,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.7.6" + "version": "3.8.8" }, "varInspector": { "cols": { diff --git a/lab06_Trajectories.ipynb b/lab06_Trajectories.ipynb index 99ff1e6..d1b1a37 100644 --- a/lab06_Trajectories.ipynb +++ b/lab06_Trajectories.ipynb @@ -11,6 +11,8 @@ }, "outputs": [], "source": [ + "# run this cell to load methods and imports\n", + "\n", "from __future__ import print_function\n", "import roboticstoolbox as rtb\n", "import matplotlib.pyplot as plt\n", @@ -296,7 +298,8 @@ "metadata": { "jupyter": { "source_hidden": true - } + }, + "scrolled": true }, "outputs": [], "source": [ @@ -421,9 +424,9 @@ " qs = np.zeros((steps,6))\n", " for i in range(steps):\n", " if i == 0:\n", - " qs[i,:] = robot.ikine_min(SE3(x[i,0],y[i,0],z[i,0])).q\n", + " qs[i,:] = robot.ikine_LM(SE3(x[i,0],y[i,0],z[i,0])).q\n", " else:\n", - " qs[i,:] = robot.ikine_min(SE3(x[i,0],y[i,0],z[i,0]), q0=qs[i-1,:]).q\n", + " qs[i,:] = robot.ikine_LM(SE3(x[i,0],y[i,0],z[i,0]), q0=qs[i-1,:]).q\n", "\n", " plot_robot(qs*180/3.141)\n", "\n", @@ -509,9 +512,9 @@ " for i in range(steps):\n", " setpoint = SE3(x[i,0],y[i,0],z[i,0])@SE3.RPY([r[i,0],p[i,0],ya[i,0]], order=\"zyx\")\n", " if i == 0:\n", - " qs[i,:] = robot.ikine_min(setpoint, q0=[0,0,0,0,0,0]).q\n", + " qs[i,:] = robot.ikine_LM(setpoint, q0=[0,0,0,0,0,0]).q\n", " else:\n", - " qs[i,:] = robot.ikine_min(setpoint, q0=qs[i-1,:]).q\n", + " qs[i,:] = robot.ikine_LM(setpoint, q0=qs[i-1,:]).q\n", "\n", " plot_robot(qs*180/3.141592)\n", "\n", @@ -580,17 +583,15 @@ "# We define our robot\n", "robot = rtb.models.URDF.UR5()\n", "\n", - "sol = robot.ikine_min(tr)\n", - "qs = np.concatenate([o.q for o in sol], axis=0)\n", - "\n", + "sol = robot.ikine_LM(tr)\n", "# working with swift backend, needs rtb.models.URDF defined robots, \n", "# opens a new tab for animation\n", - "# robot.plot(qs, backend=\"swift\")\n", + "robot.plot(sol.q, backend=\"swift\")\n", "\n", "# working with pyplot backend, needs rtb.models.DH defined robots,\n", "# animation to be saved in a gif \n", - "# robot = rtb.models.DH.UR5() \n", - "# robot.plot(qs, backend=\"pyplot\", movie=\"test.gif\")" + "#robot = rtb.models.DH.UR5() \n", + "#robot.plot(sol.q, backend=\"pyplot\", movie=\"test.gif\")" ] }, { @@ -652,7 +653,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.7.6" + "version": "3.8.8" }, "varInspector": { "cols": { diff --git a/lab07_DynamicModel.ipynb b/lab07_DynamicModel.ipynb index 4a0b676..54f86f5 100644 --- a/lab07_DynamicModel.ipynb +++ b/lab07_DynamicModel.ipynb @@ -529,7 +529,7 @@ " 2. Consider the 2DOF robotic structure from Figure 7.3, for which $L1=L2=1 \\;m$, $m1=m2=1\\;kg$. The q1 and q2 initial conditions are $\\pi$/4 and 1.\n", " \n", "\n", - " a. Find the DGM using the D-H convention and create the robot using the robopy library.\n", + " a. Find the DGM using the D-H convention and create the robot using the robotics toolbox library.\n", " b. Compute the C, D, and G matrices. \n", " c. Implement in Python the robot model with odeintw (wrapper of odeint for working with matrices), having two sine waves as the input joint torques. \n", " d. Plot the positions and velocities compared with the inputs. Interpret the results.\n", @@ -549,15 +549,6 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "scrolled": false - }, - "outputs": [], "source": [ "%reset -f\n", "import numpy as np\n", @@ -588,7 +579,7 @@ " \n", " return xdot\n", "\n", - "# initial condition od the state variable\n", + "# initial condition of the state variable\n", "x0 = [0,0]\n", "\n", "# number of time points\n", @@ -647,16 +638,7 @@ "cell_type": "code", "execution_count": null, "metadata": { - "scrolled": false - }, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "scrolled": false + "code_folding": [] }, "outputs": [], "source": [ @@ -668,26 +650,30 @@ "from roboticstoolbox import *\n", "from spatialmath import *\n", "from spatialmath.base import * \n", - "\n", + "from lab_functions import lim_prismatic\n", "from odeintw import *\n", "\n", "########## 2C, 2D ############\n", "\n", + "pr_lim = 10\n", + "\n", "def model(x,t,u):\n", " \n", " \n", " u = u.reshape(2,1) # reshaping for mathematical operations\n", " \n", - " q = x[:,0] #first column of x, the states\n", - " dq = x[:,1] #second column of x, the states derivated\n", + " q = x[:,0] #first column of states x, the positions\n", + " dq = x[:,1] #second column of states x, the velocities\n", "\n", + " lim_prismatic(q,dq,pr_lim)\n", + " \n", + " \n", " D = \n", "\n", " C =\n", " \n", " G = \n", " \n", - "\n", " xdot1 = \n", " xdot2 = \n", " \n", @@ -702,7 +688,45 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": [] + "source": [ + "# AL5D toolbox example\n", + "\n", + "import roboticstoolbox as rtb\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "from math import pi \n", + "\n", + "rob = rtb.models.DH.AL5D_mdw()\n", + "q = np.array([0,0,0,0,0])\n", + "qd = q\n", + "tau = q\n", + "\n", + "print(rob.dynamics())\n", + "\n", + "# G, M, C matrices at position and velocity q and qd\n", + "G = rob.gravload(q)\n", + "M = rob.inertia(q)\n", + "C = rob.coriolis(q, qd)\n", + "\n", + "# forward dynamics\n", + "qdd = rob.accel(q, tau, qd)\n", + "\n", + "def no_torque_func(rob, t, q, qd):\n", + " return np.zeros((rob.n, ))\n", + "\n", + "tg = rob.fdyn(T=5, q0=q, dt=0.01, torque=no_torque_func)\n", + " \n", + "plt.figure(2)\n", + "plt.plot(tg.t, tg.q[:,0], 'b', label='q1')\n", + "plt.plot(tg.t, tg.q[:,1], 'r', label='q2')\n", + "plt.plot(tg.t, tg.q[:,2], 'g', label='q3')\n", + "plt.plot(tg.t, tg.q[:,3], 'm', label='q4')\n", + "plt.plot(tg.t, tg.q[:,4], 'y', label='q5')\n", + "plt.legend(loc='best')\n", + "plt.ylabel('values')\n", + "plt.xlabel('time')\n", + "plt.grid(True)" + ] } ], "metadata": { @@ -721,7 +745,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.7.6" + "version": "3.8.8" }, "varInspector": { "cols": { diff --git a/lab10_ControlDesign.ipynb b/lab10_ControlDesign.ipynb index 21416b2..83fd623 100644 --- a/lab10_ControlDesign.ipynb +++ b/lab10_ControlDesign.ipynb @@ -480,22 +480,11 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": null, "metadata": { "code_folding": [] }, - "outputs": [ - { - "data": { - "image/png": "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\n", - "text/plain": [ - "<Figure size 1332x1332 with 4 Axes>" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], + "outputs": [], "source": [ "%reset -f\n", "import numpy as np\n", @@ -957,7 +946,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.7.6" + "version": "3.8.8" }, "varInspector": { "cols": { diff --git a/lab11_ControlDesign-Drones.ipynb b/lab11_ControlDesign-Drones.ipynb index 982763e..2dfe53c 100644 --- a/lab11_ControlDesign-Drones.ipynb +++ b/lab11_ControlDesign-Drones.ipynb @@ -818,22 +818,9 @@ }, { "cell_type": "code", - "execution_count": 21, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "image/png": "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\n", - "text/plain": [ - "<Figure size 1332x756 with 1 Axes>" - ] - }, - "metadata": { - "needs_background": "light" - }, - "output_type": "display_data" - } - ], + "outputs": [], "source": [ "import math as m \n", "import numpy as np\n", @@ -1079,7 +1066,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.7.6" + "version": "3.8.8" }, "varInspector": { "cols": { diff --git a/lab_functions.py b/lab_functions.py index 6bedbda..2bc0c1d 100644 --- a/lab_functions.py +++ b/lab_functions.py @@ -1,5 +1,5 @@ from __future__ import print_function -import serial +#import serial import sys import time @@ -147,4 +147,9 @@ def velocity_ell(q1, q2): el.angle = np.degrees(np.arccos(v[1][0])) plt.show() - \ No newline at end of file + + +def lim_prismatic(q, dq, val): + if (abs(q[1]) > val): + q[1] = val* q[1]/abs(q[1]) + dq[1] = 0 \ No newline at end of file -- GitLab From cab16f4ddc17c0c4973ac0ec3a276d1f21daa4cf Mon Sep 17 00:00:00 2001 From: Ioana Anamaria <ioana.ulici@student.utcluj.ro> Date: Tue, 11 Oct 2022 11:18:46 +0300 Subject: [PATCH 2/3] Real robot control updated --- lab02_DirectGeometricModel.ipynb | 58 +++++++++++++++++++--------- lab03_DHconvention.ipynb | 57 ++++++++++++++++++--------- lab04_Jacobian.ipynb | 66 +++++++++++++++++++++++--------- lab05_IKM.ipynb | 49 +++++++++++++++++------- lab06_Trajectories.ipynb | 24 +----------- 5 files changed, 162 insertions(+), 92 deletions(-) diff --git a/lab02_DirectGeometricModel.ipynb b/lab02_DirectGeometricModel.ipynb index 773d1f2..dd39150 100644 --- a/lab02_DirectGeometricModel.ipynb +++ b/lab02_DirectGeometricModel.ipynb @@ -518,34 +518,54 @@ "outputs": [], "source": [ "### Cell for sending comamnd to the AL5D ###\n", - "\n", "# Importing necessary modules\n", - "from __future__ import print_function\n", "import serial\n", "import sys\n", "import time\n", "from ipywidgets import interact, interactive, fixed, interact_manual\n", - "import ipywidgets as widgets\n", - "\n", - "def fkine(q0,q1,q2,q3,q4, q5): #Method for controlling the joints\n", - " pwq0 = str(round(q0*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", - " pwq1 = str(round(q1*11.1111 + 500))\n", - " pwq2 = str(round(q2*11.1111 + 500))\n", - " pwq3 = str(round(q3*11.1111 + 500))\n", - " pwq4 = str(round(q4*11.1111 + 500))\n", - " pwq5 = str(round(q5*11.1111 + 500))\n", - "\n", - " # Concatenating the desired control signals in a long string\n", - " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\"+\"#4P\" + pwq4 + \"S1500\"+\"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", - " output = output.encode('utf-8') # Converting to bytes literals\n", - " ssc32.write(output) # sending serial data to the SSC-32 board\n", - "\n", + "import roboticstoolbox as rtb\n", + "import numpy as np\n", + "\n", + "al5d = rtb.models.URDF.AL5D_mdw()\n", + "\n", + "def robot_control(q0=0, q1=0, q2=0, q3=0, q4=0, gripper=0): #Method for controlling the joints\n", + " q0r = q0*np.pi/180\n", + " q1r = q1*np.pi/180\n", + " q2r = q2*np.pi/180\n", + " q3r = q3*np.pi/180\n", + " q4r = q4*np.pi/180\n", + " elbow = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow')\n", + " wrist = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist')\n", + " grip = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')\n", + " tool = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')\n", + " \n", + " if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):\n", + " print(\"Robot collision with the table\")\n", + " else:\n", + " print(\"No collision detected\")\n", + " pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", + " pwq1 = str(round((q1+90)*11.1111 + 500))\n", + " pwq2 = str(round((q2+90)*11.1111 + 500))\n", + " pwq3 = str(round((q3+90)*11.1111 + 500))\n", + " pwq4 = str(round((q4+90)*11.1111 + 500))\n", + " pwq5 = str(round((gripper+90)*11.1111 + 500))\n", + " # Concatinating the desired control signals in a long string\n", + " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\" + \"#4P\" + pwq4 + \"S1500\"+ \"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", + " output = output.encode('utf-8') # Converting to bytes literals\n", + " ssc32.write(output) # sending serial data to the SSC-32 board\n", + "\n", + "devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9']\n", + " \n", "# Starting the connection with the SSC-32U controller\n", - "ssc32 = serial.Serial('COM3', 9600, timeout=1.0)\n", + "for device in devices:\n", + " try:\n", + " ssc32 = serial.Serial(device, 9600, timeout=1.0)\n", + " except serial.SerialException:\n", + " print(\"No device connected on \", device)\n", "# Initializing the position of the robot\n", "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", "# Initialising the sliders\n", - "interact(fkine, q0=(0,180),q1=(0,180),q2=(0,180),q3=(0,180),q4=(0,180), q5=(0,180))" + "interact(robot_control, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90))" ] }, { diff --git a/lab03_DHconvention.ipynb b/lab03_DHconvention.ipynb index f4541ea..33827f4 100644 --- a/lab03_DHconvention.ipynb +++ b/lab03_DHconvention.ipynb @@ -624,35 +624,54 @@ "outputs": [], "source": [ "### Cell for sending commands to the AL5D robot ###\n", - "\n", "# Importing necessary modules\n", - "from __future__ import print_function\n", "import serial\n", "import sys\n", "import time\n", "from ipywidgets import interact, interactive, fixed, interact_manual\n", - "import ipywidgets as widgets\n", - "\n", - "\n", - "def fkine(q0,q1,q2,q3,q4, q5): #Method for controlling the joints\n", - " pwq0 = str(round(q0*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", - " pwq1 = str(round(q1*11.1111 + 500))\n", - " pwq2 = str(round(q2*11.1111 + 500))\n", - " pwq3 = str(round(q3*11.1111 + 500))\n", - " pwq4 = str(round(q4*11.1111 + 500))\n", - " pwq5 = str(round(q5*11.1111 + 500))\n", - "\n", - " # Concatenating the desired control signals in a long string\n", - " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\"+\"#4P\" + pwq4 + \"S1500\"+\"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", - " output = output.encode('utf-8') # Converting to bytes literals\n", - " ssc32.write(output) # sending serial data to the SSC-32 board\n", + "import roboticstoolbox as rtb\n", + "import numpy as np\n", "\n", + "al5d = rtb.models.URDF.AL5D_mdw()\n", + "\n", + "def robot_control(q0=0, q1=0, q2=0, q3=0, q4=0, gripper=0): #Method for controlling the joints\n", + " q0r = q0*np.pi/180\n", + " q1r = q1*np.pi/180\n", + " q2r = q2*np.pi/180\n", + " q3r = q3*np.pi/180\n", + " q4r = q4*np.pi/180\n", + " elbow = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow')\n", + " wrist = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist')\n", + " grip = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')\n", + " tool = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')\n", + " \n", + " if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):\n", + " print(\"Robot collision with the table\")\n", + " else:\n", + " print(\"No collision detected\")\n", + " pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", + " pwq1 = str(round((q1+90)*11.1111 + 500))\n", + " pwq2 = str(round((q2+90)*11.1111 + 500))\n", + " pwq3 = str(round((q3+90)*11.1111 + 500))\n", + " pwq4 = str(round((q4+90)*11.1111 + 500))\n", + " pwq5 = str(round((gripper+90)*11.1111 + 500))\n", + " # Concatinating the desired control signals in a long string\n", + " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\" + \"#4P\" + pwq4 + \"S1500\"+ \"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", + " output = output.encode('utf-8') # Converting to bytes literals\n", + " ssc32.write(output) # sending serial data to the SSC-32 board\n", + "\n", + "devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9']\n", + " \n", "# Starting the connection with the SSC-32U controller\n", - "ssc32 = serial.Serial('COM3', 9600, timeout=1.0)\n", + "for device in devices:\n", + " try:\n", + " ssc32 = serial.Serial(device, 9600, timeout=1.0)\n", + " except serial.SerialException:\n", + " print(\"No device connected on \", device)\n", "# Initializing the position of the robot\n", "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", "# Initialising the sliders\n", - "interact(fkine, q0=(0,180),q1=(0,180),q2=(0,180),q3=(0,180),q4=(0,180), q5=(0,180))" + "interact(robot_control, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90))" ] } ], diff --git a/lab04_Jacobian.ipynb b/lab04_Jacobian.ipynb index 1883c15..67806ad 100644 --- a/lab04_Jacobian.ipynb +++ b/lab04_Jacobian.ipynb @@ -500,38 +500,66 @@ "outputs": [], "source": [ "## exercise 2 point e ##\n", - "from __future__ import print_function\n", "import serial\n", "import sys\n", "import time\n", "from ipywidgets import interact, interactive, fixed, interact_manual\n", + "import roboticstoolbox as rtb\n", "import ipywidgets as widgets\n", "\n", - "def velocities(q0,q1,q2,q3,q4, dq0,dq1,dq2,dq3): #Method for controlling the joints coordinates and velocities\n", - " pwq0 = str(round(q0*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", - " pwq1 = str(round(q1*11.1111 + 500))\n", - " pwq2 = str(round(q2*11.1111 + 500))\n", - " pwq3 = str(round(q3*11.1111 + 500))\n", - " pwq4 = str(round(q4*11.1111 + 500))\n", + "al5d = rtb.models.URDF.AL5D_mdw()\n", + "\n", + "def velocities(q0=0,q1=0,q2=0,q3=0,q4=0,gripper=0,dq0,dq1,dq2,dq3,dq4): #Method for controlling the joints coordinates and velocities\n", + " q0r = q0*np.pi/180\n", + " q1r = q1*np.pi/180\n", + " q2r = q2*np.pi/180\n", + " q3r = q3*np.pi/180\n", + " q4r = q4*np.pi/180\n", + " elbow = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow')\n", + " wrist = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist')\n", + " grip = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')\n", + " tool = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')\n", " \n", - " # Scaling of the joint velocity signal from deg/s to what the servos should receive\n", - " pwdq0 = str(round(dq0*11.1111)) \n", - " pwdq1 = str(round(dq1*11.1111)) \n", - " pwdq2 = str(round(dq2*11.1111)) \n", - " pwdq3 = str(round(dq3*11.1111)) \n", - " pwdq4 = str(150)\n", + " if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):\n", + " print(\"Robot collision with the table\")\n", + " else:\n", + " print(\"No collision detected\")\n", + " pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", + " pwq1 = str(round((q1+90)*11.1111 + 500))\n", + " pwq2 = str(round((q2+90)*11.1111 + 500))\n", + " pwq3 = str(round((q3+90)*11.1111 + 500))\n", + " pwq4 = str(round((q4+90)*11.1111 + 500))\n", + " pwq5 = str(round((gripper+90)*11.1111 + 500))\n", " \n", - " # Concatenating the desired control signals in a long string\n", - " output =\"#0P\"+pwq0+\"S\"+pwdq0+\"#1P\"+pwq1+\"S\"+pwdq1+\"#2P\"+pwq2+\"S\"+pwdq2+\"#3P\"+pwq3+\"S\"+pwdq3+\"#4P\"+pwq4+\"S\"+pwdq4+\"\\r\"# Formatting string\n", - " output = output.encode('utf-8') # Converting to bytes literals\n", - " ssc32.write(output) # sending serial data to the SSC-32 board\n", + " # Scaling of the joint velocity signal from deg/s to what the servos should receive\n", + " pwdq0 = str(round(dq0*11.1111)) \n", + " pwdq1 = str(round(dq1*11.1111)) \n", + " pwdq2 = str(round(dq2*11.1111)) \n", + " pwdq3 = str(round(dq3*11.1111)) \n", + " pwdq4 = str(round(dq4*11.1111)) \n", + " pwdq5 = str(150)\n", + "\n", + " # Concatenating the desired control signals in a long string\n", + " output =\"#0P\"+pwq0+\"S\"+pwdq0+\"#1P\"+pwq1+\"S\"+pwdq1+\"#2P\"+pwq2+\"S\"+pwdq2+\"#3P\"+pwq3+\"S\"+pwdq3+\"#4P\"+pwq4+\"S\"+pwdq4+\"#5P\"+pwq5+\"S\"+pwdq5+\"\\r\"# Formatting string\n", + " output = output.encode('utf-8') # Converting to bytes literals\n", + " ssc32.write(output) # sending serial data to the SSC-32 board\n", " \n", + "devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9']\n", + " \n", "# Starting the connection with the SSC-32U controller\n", - "ssc32 = serial.Serial('COM6', 9600, timeout=1.0)\n", + "for device in devices:\n", + " try:\n", + " ssc32 = serial.Serial(device, 9600, timeout=1.0)\n", + " except serial.SerialException:\n", + " print(\"No device connected on \", device)\n", + "\n", "# Initializing the position of the robot\n", "ssc32.write(b'#0P1500S100#1P1500S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", "# Initialising the sliders\n", - "interact_manual(velocities, q0=(0,180),q1=(0,180),q2=(0,180),q3=(0,180),q4=(0,90), dq0=(9,180),dq1=(9,180),dq2=(9,180),dq3=(9,180))\n" + "interact_manual(velocities, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90), dq0=(9,180),dq1=(9,180),dq2=(9,180),dq3=(9,180), dq4=(9,180))\n", + "\n", + "\n", + "\n" ] }, { diff --git a/lab05_IKM.ipynb b/lab05_IKM.ipynb index 10fc9a4..5968c11 100644 --- a/lab05_IKM.ipynb +++ b/lab05_IKM.ipynb @@ -379,21 +379,44 @@ "import sys\n", "import time\n", "\n", - "def al5d_fkine(q0,q1,q2,q3,q4): #Method for controlling the joints\n", - " pwq0 = str(round(q0*180*11.1111/3.141 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", - " pwq1 = str(round(q1*180*11.1111/3.141 + 500))\n", - " pwq2 = str(round(q2*180*11.1111/3.141 + 500))\n", - " pwq3 = str(round(q3*180*11.1111/3.141 + 500))\n", - " pwq4 = str(round(q4*180*11.1111/3.141 + 500))\n", - " # Concatinating the desired control signals in a long string\n", - " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\"+\"#4P\" + pwq4 + \"S1500\"+\"\\r\" # Formatting string\n", - " output = output.encode('utf-8') # Converting to bytes literals\n", - " ssc32.write(output) # sending serial data to the SSC-32 board\n", - "\n", + "def al5d_fkine(q0=0,q1=0,q2=0,q3=0,q4=0, gripper=0): #Method for controlling the joints\n", + " q0r = q0*np.pi/180\n", + " q1r = q1*np.pi/180\n", + " q2r = q2*np.pi/180\n", + " q3r = q3*np.pi/180\n", + " q4r = q4*np.pi/180\n", + " elbow = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow')\n", + " wrist = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist')\n", + " grip = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')\n", + " tool = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')\n", + " \n", + " if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):\n", + " print(\"Robot collision with the table\")\n", + " else:\n", + " print(\"No collision detected\")\n", + " pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", + " pwq1 = str(round((q1+90)*11.1111 + 500))\n", + " pwq2 = str(round((q2+90)*11.1111 + 500))\n", + " pwq3 = str(round((q3+90)*11.1111 + 500))\n", + " pwq4 = str(round((q4+90)*11.1111 + 500))\n", + " pwq5 = str(round((gripper+90)*11.1111 + 500))\n", + " # Concatinating the desired control signals in a long string\n", + " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\" + \"#4P\" + pwq4 + \"S1500\"+ \"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", + " output = output.encode('utf-8') # Converting to bytes literals\n", + " ssc32.write(output) # sending serial data to the SSC-32 board\n", + "\n", + "\n", + "devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9']\n", + " \n", "# Starting the connection with the SSC-32U controller\n", - "ssc32 = serial.Serial('COM3', 9600, timeout=1.0)\n", + "for device in devices:\n", + " try:\n", + " ssc32 = serial.Serial(device, 9600, timeout=1.0)\n", + " except serial.SerialException:\n", + " print(\"No device connected on \", device)\n", + " \n", "# Initializing the position of the robot\n", - "ssc32.write(b'#0P1500S300#1P1500S300#2P1500S300#3P1500S300#4P1500S1500#5P1500S1500\\r')\n", + "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", "\n", "# Import AL5D model\n", "\n", diff --git a/lab06_Trajectories.ipynb b/lab06_Trajectories.ipynb index d1b1a37..4d551d7 100644 --- a/lab06_Trajectories.ipynb +++ b/lab06_Trajectories.ipynb @@ -4,9 +4,6 @@ "cell_type": "code", "execution_count": null, "metadata": { - "jupyter": { - "source_hidden": true - }, "tags": [] }, "outputs": [], @@ -149,9 +146,6 @@ "cell_type": "code", "execution_count": null, "metadata": { - "jupyter": { - "source_hidden": true - }, "tags": [] }, "outputs": [], @@ -251,9 +245,6 @@ "cell_type": "code", "execution_count": null, "metadata": { - "jupyter": { - "source_hidden": true - }, "tags": [] }, "outputs": [], @@ -296,10 +287,8 @@ "cell_type": "code", "execution_count": null, "metadata": { - "jupyter": { - "source_hidden": true - }, - "scrolled": true + "scrolled": true, + "tags": [] }, "outputs": [], "source": [ @@ -354,9 +343,6 @@ "cell_type": "code", "execution_count": null, "metadata": { - "jupyter": { - "source_hidden": true - }, "tags": [] }, "outputs": [], @@ -395,9 +381,6 @@ "cell_type": "code", "execution_count": null, "metadata": { - "jupyter": { - "source_hidden": true - }, "tags": [] }, "outputs": [], @@ -476,9 +459,6 @@ "cell_type": "code", "execution_count": null, "metadata": { - "jupyter": { - "source_hidden": true - }, "tags": [] }, "outputs": [], -- GitLab From 05e5d0435f66664c9608396fb867d9a9b71cc734 Mon Sep 17 00:00:00 2001 From: Ioana Anamaria <ioana.ulici@student.utcluj.ro> Date: Tue, 11 Oct 2022 12:54:51 +0300 Subject: [PATCH 3/3] Shelter code for running the al5d real robot in its own class and just call methods in the notebooks --- al5d_control.ipynb | 51 ++++------------------------------------- al5d_control.py | 56 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 60 insertions(+), 47 deletions(-) create mode 100644 al5d_control.py diff --git a/al5d_control.ipynb b/al5d_control.ipynb index 0c3f584..383e7ff 100644 --- a/al5d_control.ipynb +++ b/al5d_control.ipynb @@ -19,54 +19,11 @@ "metadata": {}, "outputs": [], "source": [ - "# Importing necessary modules\n", - "import serial\n", - "import sys\n", - "import time\n", - "from ipywidgets import interact, interactive, fixed, interact_manual\n", - "import roboticstoolbox as rtb\n", - "import numpy as np\n", + "from al5d_control import *\n", "\n", - "al5d = rtb.models.URDF.AL5D_mdw()\n", - "\n", - "def robot_control(q0=0, q1=0, q2=0, q3=0, q4=0, gripper=0): #Method for controlling the joints\n", - " q0r = q0*np.pi/180\n", - " q1r = q1*np.pi/180\n", - " q2r = q2*np.pi/180\n", - " q3r = q3*np.pi/180\n", - " q4r = q4*np.pi/180\n", - " elbow = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow')\n", - " wrist = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist')\n", - " grip = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')\n", - " tool = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')\n", - " \n", - " if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):\n", - " print(\"Robot collision with the table\")\n", - " else:\n", - " print(\"No collision detected\")\n", - " pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n", - " pwq1 = str(round((q1+90)*11.1111 + 500))\n", - " pwq2 = str(round((q2+90)*11.1111 + 500))\n", - " pwq3 = str(round((q3+90)*11.1111 + 500))\n", - " pwq4 = str(round((q4+90)*11.1111 + 500))\n", - " pwq5 = str(round((gripper+90)*11.1111 + 500))\n", - " # Concatinating the desired control signals in a long string\n", - " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\" + \"#4P\" + pwq4 + \"S1500\"+ \"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n", - " output = output.encode('utf-8') # Converting to bytes literals\n", - " ssc32.write(output) # sending serial data to the SSC-32 board\n", - "\n", - "devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9']\n", - " \n", - "# Starting the connection with the SSC-32U controller\n", - "for device in devices:\n", - " try:\n", - " ssc32 = serial.Serial(device, 9600, timeout=1.0)\n", - " except serial.SerialException:\n", - " print(\"No device connected on \", device)\n", - "# Initializing the position of the robot\n", - "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n", - "# Initialising the sliders\n", - "interact(robot_control, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90))" + "rrob = AL5DControl()\n", + "rrob.send_commands()\n", + "\n" ] } ], diff --git a/al5d_control.py b/al5d_control.py new file mode 100644 index 0000000..5b27367 --- /dev/null +++ b/al5d_control.py @@ -0,0 +1,56 @@ +# Importing necessary modules +import serial +import sys +import time +from ipywidgets import interact, interactive, fixed, interact_manual +import roboticstoolbox as rtb +import numpy as np + +class AL5DControl: + def __init__(self): + + # al5d model + self.al5d = rtb.models.URDF.AL5D_mdw() + + # search for port, initialise in home pose + devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9'] + + # Starting the connection with the SSC-32U controller + for device in devices: + try: + self.ssc32 = serial.Serial(device, 9600, timeout=1.0) + # Initializing the position of the robot + self.ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\r') + except serial.SerialException: + print("No device connected on ", device) + + def robot_control(self, q0=0, q1=0, q2=0, q3=0, q4=0, gripper=0): #Method for controlling the joints + q0r = q0*np.pi/180 + q1r = q1*np.pi/180 + q2r = q2*np.pi/180 + q3r = q3*np.pi/180 + q4r = q4*np.pi/180 + elbow = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow') + wrist = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist') + grip = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper') + tool = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool') + + if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06): + print("Robot collision with the table") + else: + print("No collision detected") + pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500) + pwq1 = str(round((q1+90)*11.1111 + 500)) + pwq2 = str(round((q2+90)*11.1111 + 500)) + pwq3 = str(round((q3+90)*11.1111 + 500)) + pwq4 = str(round((q4+90)*11.1111 + 500)) + pwq5 = str(round((gripper+90)*11.1111 + 500)) + # Concatinating the desired control signals in a long string + output = "#0P" + pwq0 + "S300"+"#1P" + pwq1 + "S300"+"#2P" + pwq2 + "S300"+"#3P" + pwq3 + "S1500" + "#4P" + pwq4 + "S1500"+ "#5P" + pwq5 + "S1500"+"\r" # Formatting string + output = output.encode('utf-8') # Converting to bytes literals + self.ssc32.write(output) # sending serial data to the SSC-32 board + + def send_commands(self): + # Initialising the sliders + interact(self.robot_control, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90)) + -- GitLab