├── .gitattributes ├── .gitignore ├── .vscode └── settings.json ├── LICENSE ├── README.md ├── arduino_sketches └── easyEEZYbotARM_Arduino_Communication │ └── easyEEZYbotARM_Arduino_Communication.ino ├── docs └── kinematics │ ├── Forward_kinematics_EEZYbotARM_v2.pdf │ ├── Inverse_kinematics_EEZYbotARM.pdf │ └── README.md ├── examples ├── example1_plot.py ├── example2_forwardKinematics.py ├── example3_inverseKinematics.py ├── example4_make_the_EEZYbotARM_move.py ├── example5_connect_kinematic_model_to_EEZYbotARM.py └── example6.py ├── fritzing_files ├── EEZYbotARM_PCA9685_v1.fzz └── EEZYbotARM_PCA9685_v1_bb.png ├── images ├── forwardKinematics.png ├── fritzingDiagram.png ├── introPicture.png ├── inverseKinematics.png ├── plottingAndWorkspace.png ├── servo_installation_position.png ├── servo_one_calibration_example.png └── softwareArchitecture.png ├── python_packages └── easyEEZYbotARM │ ├── __init__.py │ ├── kinematic_model.py │ └── serial_communication.py ├── requirements.txt ├── resources └── example_servo_calibration.xlsx └── setup.py /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # celery beat schedule file 95 | celerybeat-schedule 96 | 97 | # SageMath parsed files 98 | *.sage.py 99 | 100 | # Environments 101 | .env 102 | .venv 103 | env/ 104 | venv/ 105 | ENV/ 106 | env.bak/ 107 | venv.bak/ 108 | 109 | # Spyder project settings 110 | .spyderproject 111 | .spyproject 112 | 113 | # Rope project settings 114 | .ropeproject 115 | 116 | # mkdocs documentation 117 | /site 118 | 119 | # mypy 120 | .mypy_cache/ 121 | .dmypy.json 122 | dmypy.json 123 | 124 | # Pyre type checker 125 | .pyre/ 126 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.pythonPath": "C:\\Users\\Ben\\Anaconda3\\envs\\EEZybot\\python.exe" 3 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 meisben 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # easyEEZYbotARM 2 | 3 | ### [Version 0.3-alpha](https://github.com/meisben/easyEEZYbotARM/releases) 4 | ![GitHub](https://img.shields.io/github/license/meisben/easyEEZYbotARM?style=flat-square) 5 | ![Badge Hit Counter](https://visitor-badge.laobi.icu/badge?page_id=meisben.easyEEZYbotARM) 6 | [![Love and Peace](http://love-and-peace.github.io/love-and-peace/badges/base/v1.0.svg)](https://github.com/love-and-peace/love-and-peace/blob/master/versions/base/v1.0/en.md) 7 | [![Stand With Ukraine](https://raw.githubusercontent.com/vshymanskyy/StandWithUkraine/main/badges/StandWithUkraine.svg)](https://stand-with-ukraine.pp.ua) 8 | 9 | 10 | A python and arduino controller for the EEZYbotARM Mk1 & Mk2 (Includes 3-D kinematics). 11 | 12 | EEZYbotARMs are a range of four-axis, open source 3d-printable robot arms designed by Carlo Franciscone. 13 | 14 | ![introPicture](images/introPicture.png) 15 | 16 | ## Contents 17 | 18 | - Introduction 19 | - Overview 20 | - Kinematic calculations 21 | - Installation 22 | - Configuration(of the physical arduino setup) 23 | - Guidance on use 24 | - Help and updates 25 | 26 | ## Introduction 27 | 28 | This code library provides functionality to control the EEZYbotARM (Mk1, Mk2) in 3-D space. Specifically its functionality includes: 29 | 30 | 1. 3-D Forward kinematics 31 | 2. 3-D Inverse kinematics 32 | 3. Plotting of the (simulated) robot arm 33 | 4. Plotting of the (simulated) workspace of the robot arm 34 | 5. Communicating with a (arduino) microcontroller 35 | 6. Using the (arduino) microcontroller to control the movement of the robot arm servo motors 36 | 37 | ==Important!== Please note this is a hobby robot arm. It should be used for learning projects only. I.e. don't expect it to pick up anything heavier than 5 grams (e.g. a glass marble). 38 | 39 | ## Overview 40 | 41 | ### Forward Kinematics 42 | The code library allows the user to easily map joint angles to an x,y,z co-ordinates, so that we know the position of the end effector 43 | 44 | The mathematical calculations for the forward kinematics can be found [here](docs/kinematics). 45 | ![forwardKinematics](images/forwardKinematics.png) 46 | 47 | ### Inverse Kinematics 48 | The code library allows the user to easily map x,y,z co-ordinates to joint angles, so that the EEZYbotARM can be controlled in 3-D space 49 | 50 | The mathematical calculations for the inverse kinematics can be found [here](docs/kinematics). 51 | ![inverseKinematics](images/inverseKinematics.png) 52 | 53 | ### Simulation the robot arm, and plotting the robot arm workspace 54 | The code library allows the user to easily plot a (simulated) robot arm. It is easy to calculate and plot the robot arm workspace. As well as checking whether a cartesian (x,y,z) point is within or outside the workspace. 55 | ![plottingAndWorkspace](images/plottingAndWorkspace.png) 56 | 57 | ## Installation 58 | This code library is distributed with python and arduino components. The python code is responsible for high level kinematics calculations and simulation. The arduino code is responsbile for low level servo control. 59 | 60 | ### Installation (python) 61 | 62 | This python code is distributed as a python package. A setup file is included and so dependent libraries should install with the package. :) 63 | 64 | To install the package on Windows, OS X or Linux: 65 | 66 | 1. Download the easyEZZYbotARM zip file or clone the github repository 67 | 2. [Optional] Create a virtual environment (e.g. using conda or venv), then activate it in a terminal window 68 | 3. Then open a terminal window and navigate to the setup.py directory using the 'cd' command 69 | 4. Install the package using the pip command: 70 | 71 | ```sh 72 | pip install -e . 73 | ``` 74 | 75 | This will create an editable install of the easyEZZYbotARM library 76 | 77 | A future aim is to distribute this module using pip 78 | 79 | ### Installation (arduino) 80 | 81 | To install the arduino code it is as simple as uploading the sketch 'arduino_sketches\easyEEZYbotARM_Arduino_Communication' to the arduino microcontroller. 82 | 83 | The easiest way to do this is using the arduino IDE, see instructions at: https://www.arduino.cc/en/Main/Software 84 | 85 | The arduino software uses ArminJo's ServoEasing library so that the motions of the servo motors are smooth and not sudden 'jolts'. **Important:** You must uncomment the line `#define USE_PCA9685_SERVO_EXPANDER` in the file *ServoEasing.h* (as per the instructions for ServoEasing). The file *ServoEasing.h* can be found in the [Arduino libraries directory](https://www.arduino.cc/en/guide/libraries). 86 | 87 | ## Configuration (of the physical arduino setup) 88 | 89 | This is the reference setup using the 'easyEZZYbotARM' repository. The servo motors represent the servo motors mounted on the EEZYbotARM. The PCA9685 is a widely available low cost board designed by Adafruit. The battery pack could be replaced by any DC power source < 6 volts. 90 | 91 | ![fritzingDiagram](images/fritzingDiagram.png) 92 | This diagram is created using Fritzing. The Fritzing files can be found at '/fritzing_files' 93 | 94 | ## Building the EEZYbotARM 95 | 96 | - Instructions for building a EEZYbotARM can be found here: http://www.eezyrobots.it/eba_mk2.html 97 | - Chris Riley has a great step by step video of the EEZYbotARM mk2 on youtube: https://www.youtube.com/watch?v=R2MI-tpXyS4 98 | - If you are looking for the googly eyes extension you can find it here!: https://www.thingiverse.com/thing:4370985 99 | 100 | **Important:** Please read the next *Calibrating the EEZYbotARM* section before you install servo motors into the robot arm. 101 | 102 | ## Calibrating the EEZYbotARM 103 | 104 | For us to be able to control the robot arm, it's really important that the servo motors are installed in **known** 'reference positions' and that we understand how they behave. This is a two step process ! 105 | 106 | ### Servo installation position procedure 107 | 108 | The servo motors (for angles q1, q2, q3) should be installed following the instructions below. 109 | 110 | 1. Command the servo motor angles to 90 degrees using an Arduino script which sends PWM (Pulse width modulation) signals to the servo. You can use the [ServoEasing](https://github.com/ArminJo/ServoEasing) library for this purpose. 111 | 2. Position the EEZYbotARM links as shown in the diagram below 112 | 3. Attach the servo arms (the bits of plastic that attach to the motor) and install the motors so that they remain in the 90 degree position, when the EEZYbotARM links are in the position below. 113 | 114 | The aim of this installation procedure is to install the servo motors in a known reference position. 115 | 116 | ![servo_installation_position](images/servo_installation_position.png) 117 | 118 | ### Improving the accuracy of the robot arm (or troubleshooting positional accuracy) 119 | 120 | **Please note:** *what is described in this sub-section is only currently implemented for the q1 servo motor in the easyEZZYbotARM library based on experimental values for a test robot. This is because it is strictly necessary for the q1 (base) servo motor. You can edit this for your particular motors (q2,q3) in the `map_kinematicsToServoAngles` function in the easyEEZYbotARM python library. Doing so will improve the positional accuracy of the robot arm* 121 | 122 | Because the robot arm is using *'hobby servo motors'*, they don't always behave like we expect them to! For example, say we command a hobby servo (using pulse width modulation) to travel to an angle of 120 degrees, and then move to an angle of 70 degrees. If we measure these angles we are likely to find they are quite a way out! This makes a big difference to how the robot arm moves, so we need to correct any errors before we install the servo motors. An example of a setup to do this is shown in the video below 123 | 124 | [![Servo calibration example (arduino)](https://img.youtube.com/vi/GVBDZIZc2mw/0.jpg)](https://www.youtube.com/watch?v=GVBDZIZc2mw) 125 | 126 | A reasonable way to do this is to command the servo motor to positions in increments of 10 degrees, and to measure the actual position of the motor. This can be recorded in a spreadsheet, so that we can map between a (demanded) input and an (actual) output using a linear function. An example of this (for the robot arm base servo motor) is given in this [excel sheet](resources/example_servo_calibration.xlsx). The graph produced from this is shown here. 127 | 128 | ![servo_one_calibration_example](images/servo_one_calibration_example.png) 129 | 130 | A [linear relationship](https://www.mathsisfun.com/equation_of_line.html) can be estimated from this data (using excel line of best fit). The equation for this is displayed on the chart. You can see that this exact equation is implemented in the easyEEZYbotARM libary, this mapping between the (demanded) input and an (actual) output is implemented (for the q1 base servo motor) as follows: 131 | 132 | ```python 133 | # Calculate for q1 134 | servoAngle_q1 = ((-2.0497)*q1) + 91.726 # from experimentation ! 135 | servoAngle_q1 = round(servoAngle_q1, 2) 136 | ``` 137 | 138 | I haven't experimented with lots of hobby servo motors, but I suspect that they are all a bit different in their performance between demanded value, and actual output. If your robot arm isn't quite in the right position (but almost is), then this could be something to optimise! 139 | 140 | ## Guidance on use 141 | 142 | ### Examples 143 | 144 | - For examples of basic usage see the '/examples' folder 145 | 146 | ### Software Architecture 147 | ![softwareArchitecture](images/softwareArchitecture.png) 148 | 149 | 150 | ## Bug list 151 | 152 | - Currently no bugs are listed here 153 | 154 | ## Help and updates 155 | 156 | If you need help using this code libray, please in the first instance try googling your problem to see if you can find a solution. If that doesn't help then please do feel free to initiate a pull request on github. 157 | 158 | ## Contributing 159 | 160 | Anyone is very welcome to contribute - please do. Below is a list of identified improvements. 161 | 162 | Some useful best practice guidelines are here: https://opensource.guide/how-to-contribute/ 163 | 164 | ### To do (improvements) 165 | 166 | - Kinematic Equations: These will be documented in a seperate web post 167 | - Add example for plotting workspace 168 | - Add instructions for installing servo motors and calibrating 169 | - Make plot (simulation) of virtual robot arm non blocking 170 | - Tidy up code 171 | - Seperate plotting function and D-H functions for calculating joint frame and link positions 172 | 173 | ## Limitations 174 | 175 | - Because the servo motors are 'hobby motors' without position feedback to the python library it's not possible to create 'straight line' type trajectories using the EEZYbotARM. If you are looking for this functionality (outside of a hobby project) I suggest using a robot arm which already implements this out of the box. For example the niryo one arm or the dobot magician arm. 176 | - Because the servo motors are 'hobby motors' they may heat up after prolonged use and loose acurracy. This depends on what activity they are undertaking, but is likely to ocurr when lifting any kind of load that isn't 'very light'. 177 | 178 | ## Thanks and credit 179 | 180 | - Thanks to you for reading this and considering using this code library 181 | - A big thanks to Carlo Franciscone for open sourcing the [EEZYbotARM](http://www.eezyrobots.it/eba_mk2.html) 182 | - A big thanks to ArminJo for creating the fantastic arduino [ServoEasing library](https://github.com/ArminJo/ServoEasing) 183 | - A big thanks to Dr Antonia Tzemanaki at the University of Bristol / University of the West of England robotics course, what an amazing teacher. 184 | - Thanks to Professor Angela Sodemann and Professor Peter Corke for putting their wonderful courses on inverse and forward kinematics online. You can find these courses [here](https://www.youtube.com/playlist?list=PLT_0lwItn0sAfi3o4xwx-fNfcnbfMrXa7) and [here](https://robotacademy.net.au/) 185 | 186 | ## Release history 187 | 188 | ### v0.3-alpha 189 | 190 | #### Documentation Updates 191 | * Added details of [kinematic calculations](https://github.com/meisben/easyEEZYbotARM/tree/master/docs/kinematics) 192 | #### Code updates 193 | * [Update to reflect newer ServoEasing library include call (.hpp)](https://github.com/meisben/easyEEZYbotARM/commit/74fdef2b7e2b1e409cfac3729df992d284b2d6d2) 194 | 195 | ### v0.2-alpha 196 | 197 | Added information and resources on calibration and installation positions for the servo motors 198 | 199 | ### v0.1-alpha 200 | 201 | Initial library version 202 | 203 | -------------------------------------------------------------------------------- /arduino_sketches/easyEEZYbotARM_Arduino_Communication/easyEEZYbotARM_Arduino_Communication.ino: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | ~~~~~Prerequisites~~~~~ 4 | ServoEasing.h (https://github.com/ArminJo/ServoEasing) -> Just a fantastic library! wow :) 5 | 6 | ~~~~~Details~~~~~~~~~~~~ 7 | Program title: EEZYbotARM control 8 | Author: Ben Money-Coomes (ben.money@gmail.com) 9 | Date: 20/11/19 10 | Purpose: Communicate with Python program for EEZYbotARM, move the arm 11 | 12 | IMPORTANT: This program receives data over the serial monitor. This data must be in the form of: 13 | "" 14 | Where this is the equivalent of 15 | This is interpreted as 16 | 17 | ~~~~~Licence~~~~~~~~~~~~ 18 | MIT License 19 | 20 | ~~~~~Read me~~~~~~~~~~~~ 21 | Can be found at https://github.com/meisben/easyEEZYbotARM 22 | 23 | ~~~~~Version Control~~~ 24 | v1.00 - exactly the same implementation as Robin2 code on Arduino forums 25 | v2.00 - no change, version control added. Works with same version of python program 26 | v3.00 - Beginning to take structure from 'servo easing' program -> BUT ONLY FOR COMMS 27 | v3.10 - Now including servo motor and buzzer control elements -> All servos working ! :) 28 | v3.20 - Changing order of angles so that end effector becomes pin0 29 | */ 30 | 31 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 32 | Library Includes. * 33 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 34 | 35 | // include these libraries for using the servo add on board. Taken from servo example code 36 | #include 37 | // include this library for servo easing functionality 38 | #include "ServoEasing.hpp" 39 | 40 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 41 | Definitions * 42 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 43 | 44 | #define VERSION "3.1" 45 | #define BUZZER_PIN 5 // buzzer to featherwing pin 5, 470 ohm resistor 46 | #define ACTION_TIME_PERIOD 1000 47 | const int SERVO1_PIN = 1; // servo pin for joint 1 48 | const int SERVO2_PIN = 2; // servo pin for joint 2 49 | const int SERVO3_PIN = 3; // servo pin for joint 3 50 | const int SERVO0_PIN = 0; // servo pin for end effector 51 | 52 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 53 | Variables * 54 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 55 | 56 | //-------- Variables for receiving serial data ------------- 57 | const byte buffSize = 40; 58 | char inputBuffer[buffSize]; 59 | const char startMarker = '<'; 60 | const char endMarker = '>'; 61 | byte bytesRecvd = 0; 62 | boolean readInProgress = false; 63 | boolean newDataFromPC = false; 64 | char messageFromPC[buffSize] = {0}; 65 | 66 | // -------- Variables to hold time ------------- 67 | unsigned long curMillis; // Variable for current time 68 | // unsigned long general_timer; 69 | 70 | // -------- Variables to hold the parsed data ------------- 71 | float floatFromPC0 = 90.0; // initial values are mid range for joint angles 72 | float floatFromPC1 = 90.0; 73 | float floatFromPC2 = 90.0; 74 | float floatFromPC3 = 90.0; 75 | int intFromPC0 = 1000; // inital values are acceptable movement times 76 | int intFromPC1 = 1000; 77 | int intFromPC2 = 1000; 78 | int intFromPC3 = 1000; 79 | float last_servoAngle_q1 = floatFromPC1; // initial values are mid range for joint angles 80 | float last_servoAngle_q2 = floatFromPC2; 81 | float last_servoAngle_q3 = intFromPC3; 82 | float last_servoAngle_EE = floatFromPC0; 83 | 84 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 85 | Instatiate clasess for libraries * 86 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 87 | ServoEasing Servo1(PCA9685_DEFAULT_ADDRESS, &Wire); 88 | ServoEasing Servo2(PCA9685_DEFAULT_ADDRESS, &Wire); 89 | ServoEasing Servo3(PCA9685_DEFAULT_ADDRESS, &Wire); 90 | ServoEasing Servo0(PCA9685_DEFAULT_ADDRESS, &Wire); 91 | 92 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 93 | START OF PROGRAM (Setup) * 94 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 95 | 96 | void setup() 97 | { 98 | // flash LED so we know we are alive 99 | blinkLED(); 100 | 101 | // Setup pins 102 | pinMode(LED_BUILTIN, OUTPUT); // setup built in LED for flashing 103 | pinMode(BUZZER_PIN, OUTPUT); // Set buzzer pin as an output 104 | 105 | // Play tone so we can tell if the board accidently reset 106 | playTone(); 107 | 108 | // Begin serial communications 109 | Serial.begin(9600); 110 | 111 | // Wait for serial communications to start before continuing 112 | while (!Serial) 113 | ; // delay for Leonardo 114 | 115 | // Just to know which program is running on my Arduino 116 | Serial.println(F("START " __FILE__ "\r\nVersion " VERSION " from " __DATE__)); 117 | 118 | // Attach servo to pin 119 | Servo1.attach(SERVO1_PIN); 120 | Servo2.attach(SERVO2_PIN); 121 | Servo3.attach(SERVO3_PIN); 122 | Servo0.attach(SERVO0_PIN); 123 | 124 | // Set servo to start position. 125 | Servo1.setEasingType(EASE_CUBIC_IN_OUT); 126 | Servo2.setEasingType(EASE_CUBIC_IN_OUT); 127 | Servo3.setEasingType(EASE_CUBIC_IN_OUT); 128 | Servo0.setEasingType(EASE_CUBIC_IN_OUT); // end effector 129 | 130 | Servo1.write(last_servoAngle_q1); 131 | Servo2.write(last_servoAngle_q2); 132 | Servo3.write(last_servoAngle_q3); 133 | Servo0.write(last_servoAngle_EE); // end effector 134 | 135 | // Just wait for servos to reach position 136 | delay(500); // delay() is OK in setup as it only happens once 137 | 138 | // tell the PC we are ready 139 | Serial.println(""); 140 | } 141 | 142 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 143 | MAIN PROGRAM (Loop) * 144 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 145 | 146 | void loop() 147 | { 148 | // This part of the loop for the serial communication is not inside a timer -> it happens very quickly 149 | curMillis = millis(); // get current time 150 | getDataFromPC(); // receive data from PC and save it into inputBuffer 151 | 152 | // need if statement -> flag to say if new data is available 153 | if (newDataFromPC == true) 154 | { 155 | processMessageFromPC(); // Processes text message from PC 156 | actionInstructionsFromPC(); // Arrange for things to move, beep, light up 157 | replyToPC(); // Reply to PC 158 | } 159 | 160 | // // This part of the loop is inside a timer -> maybe delete 161 | // unsigned long elapsed_time_general_timer = millis() - general_timer; 162 | // 163 | // if ( elapsed_time_general_timer > ACTION_TIME_PERIOD ) // set a target for the Romi periodically 164 | // { 165 | // // update timestamp 166 | // general_timer = millis(); 167 | // } 168 | } 169 | 170 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 171 | FUNCTIONS FOR MAKING THINGS MOVE OR LIGHT UP OR BEEP! * 172 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 173 | 174 | //~~~~~~~~~~~~~Fuction: Blink LED~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 175 | 176 | void blinkLED() 177 | { 178 | digitalWrite(LED_BUILTIN, HIGH); 179 | delay(100); 180 | digitalWrite(LED_BUILTIN, LOW); 181 | delay(100); 182 | } 183 | 184 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 185 | 186 | //~~~~~~~~~~~~~Fuction: Play Buzzer Tone~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 187 | 188 | void playTone() 189 | { 190 | tone(BUZZER_PIN, 650, 300); 191 | } 192 | 193 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 194 | 195 | //~~~~~~~~~~~~~Fuction: Action the instructions from the PC~~~~~~~~~~~~~~~~~~~~~~~ 196 | 197 | void actionInstructionsFromPC() 198 | { 199 | // Local variables 200 | // -- joint angles 201 | float servoAngle_q1 = floatFromPC1; 202 | float servoAngle_q2 = floatFromPC2; 203 | float servoAngle_q3 = floatFromPC3; 204 | float servoAngle_EE = floatFromPC0; 205 | // -- joint speeds 206 | int servoTime_q1 = intFromPC1; 207 | int servoTime_q2 = intFromPC2; 208 | int servoTime_q3 = intFromPC3; 209 | int servoTime_EE = intFromPC0; 210 | 211 | // Check if the joint angle has changed! 212 | if (servoAngle_q1 != last_servoAngle_q1) 213 | { 214 | Serial.println(F("Servo 1 moving to position using interrupts")); 215 | 216 | Servo1.startEaseToD(servoAngle_q1, servoTime_q1); 217 | 218 | // while (Servo3.isMovingAndCallYield()) { 219 | // ; // no delays here to avoid break between forth and back movement 220 | // } 221 | } 222 | 223 | if (servoAngle_q2 != last_servoAngle_q2) 224 | { 225 | Serial.println(F("Servo 2 moving to position using interrupts")); 226 | 227 | Servo2.startEaseToD(servoAngle_q2, servoTime_q2); 228 | 229 | // while (Servo3.isMovingAndCallYield()) { 230 | // ; // no delays here to avoid break between forth and back movement 231 | // } 232 | } 233 | 234 | if (servoAngle_q3 != last_servoAngle_q3) 235 | { 236 | Serial.println(F("Servo 3 moving to position using interrupts")); 237 | 238 | Servo3.startEaseToD(servoAngle_q3, servoTime_q3); 239 | 240 | // while (Servo3.isMovingAndCallYield()) { 241 | // ; // no delays here to avoid break between forth and back movement 242 | // } 243 | } 244 | 245 | if (servoAngle_EE != last_servoAngle_EE) 246 | { 247 | Serial.println(F("Servo EE moving to position using interrupts")); 248 | 249 | Servo0.startEaseToD(servoAngle_EE, servoTime_EE); 250 | 251 | // while (Servo3.isMovingAndCallYield()) { 252 | // ; // no delays here to avoid break between forth and back movement 253 | // } 254 | } 255 | 256 | // Store current joint angle 257 | last_servoAngle_q1 = servoAngle_q1; 258 | last_servoAngle_q2 = servoAngle_q2; 259 | last_servoAngle_q3 = servoAngle_q3; 260 | last_servoAngle_EE = servoAngle_EE; 261 | } 262 | 263 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 264 | 265 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 266 | FUNCTIONS FOR RECEIVING DATA VIA SERIAL MONITOR * 267 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 268 | 269 | //~~~~~~~~~~~~~Fuction: Receive data with start and end markers~~~~~~~~~~~~~~~~~~ 270 | 271 | void getDataFromPC() 272 | { 273 | 274 | // This function receives data from PC and saves it into inputBuffer 275 | 276 | if (Serial.available() > 0 && newDataFromPC == false) 277 | { 278 | 279 | char x = Serial.read(); 280 | 281 | // the order of these IF clauses is significant 282 | 283 | if (x == endMarker) 284 | { 285 | readInProgress = false; 286 | newDataFromPC = true; 287 | inputBuffer[bytesRecvd] = 0; 288 | parseData(); 289 | } 290 | 291 | if (readInProgress) 292 | { 293 | inputBuffer[bytesRecvd] = x; 294 | bytesRecvd++; 295 | if (bytesRecvd == buffSize) 296 | { 297 | bytesRecvd = buffSize - 1; 298 | } 299 | } 300 | 301 | if (x == startMarker) 302 | { 303 | bytesRecvd = 0; 304 | readInProgress = true; 305 | } 306 | } 307 | } 308 | 309 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 310 | 311 | //~~~~~~~~~~~~~Fuction: Split data into known component parts~~~~~~~~~~~~~~~~~~ 312 | 313 | void parseData() 314 | { 315 | 316 | // split the data into its parts 317 | 318 | char *strtokIndx; // this is used by strtok() as an index 319 | 320 | strtokIndx = strtok(inputBuffer, ","); // get the first part - the string 321 | strcpy(messageFromPC, strtokIndx); // copy it to messageFromPC 322 | 323 | strtokIndx = strtok(NULL, ","); // this continues where the previous call left off 324 | floatFromPC0 = atof(strtokIndx); // convert this part to a float -> to convert to an integer we would use atoi 325 | 326 | strtokIndx = strtok(NULL, ","); 327 | floatFromPC1 = atof(strtokIndx); // convert this part to a float 328 | 329 | strtokIndx = strtok(NULL, ","); 330 | floatFromPC2 = atof(strtokIndx); // convert this part to a float 331 | 332 | strtokIndx = strtok(NULL, ","); 333 | floatFromPC3 = atof(strtokIndx); // convert this part to a float 334 | 335 | strtokIndx = strtok(NULL, ","); 336 | intFromPC0 = atoi(strtokIndx); // convert this part to a int 337 | 338 | strtokIndx = strtok(NULL, ","); 339 | intFromPC1 = atoi(strtokIndx); // convert this part to a int 340 | 341 | strtokIndx = strtok(NULL, ","); 342 | intFromPC2 = atoi(strtokIndx); // convert this part to a int 343 | 344 | strtokIndx = strtok(NULL, ","); 345 | intFromPC3 = atoi(strtokIndx); // convert this part to a int 346 | } 347 | 348 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 349 | 350 | //~~~~~~~~~~~~~Fuction: Send message back to PC~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 351 | 352 | void replyToPC() 353 | { 354 | 355 | if (newDataFromPC) 356 | { 357 | newDataFromPC = false; 358 | Serial.print(F("")); 379 | } 380 | } 381 | 382 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 383 | 384 | //~~~~~~~~~~~~~Fuction: Process the string message from the PC~~~~~~~~~~~~~~~~~~~~ 385 | 386 | void processMessageFromPC() 387 | { 388 | 389 | // this illustrates using different inputs to call different functions 390 | // strcmp compares two strings and returns zero if the strings are equal 391 | 392 | if (strcmp(messageFromPC, "LED") == 0) 393 | { 394 | blinkLED(); 395 | } 396 | 397 | if (strcmp(messageFromPC, "BUZZ") == 0) 398 | { 399 | playTone(); 400 | } 401 | 402 | if (strcmp(messageFromPC, "BUZZ") == 0) 403 | { 404 | playTone(); 405 | } 406 | } 407 | 408 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 409 | -------------------------------------------------------------------------------- /docs/kinematics/Forward_kinematics_EEZYbotARM_v2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/docs/kinematics/Forward_kinematics_EEZYbotARM_v2.pdf -------------------------------------------------------------------------------- /docs/kinematics/Inverse_kinematics_EEZYbotARM.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/docs/kinematics/Inverse_kinematics_EEZYbotARM.pdf -------------------------------------------------------------------------------- /docs/kinematics/README.md: -------------------------------------------------------------------------------- 1 | # Kinematics 2 | 3 | ## Introduction 4 | 5 | Kinematics is the mapping of joint angles to the end effector position. It allows us to calculate how to control the robot arm. 6 | 7 | ## Generalisation 8 | 9 | The method of these kinematic calculations should generalise to other similar geometry (4 axis) robot arms. 10 | 11 | For example: 12 | 13 | * [Dobot Magician](https://www.dobot-robots.com/products/education/magician.html) 14 | * [Dobot MG400](https://www.dobot-robots.com/products/education/magician.html) 15 | * [MyPalletizer 260 M5Stack](https://shop.elephantrobotics.com/en-gb/products/mypalletizer) 16 | 17 | ## Forward kinematic calculations 18 | 19 | The [forward kinematic calculations](Forward_kinematics_EEZYbotARM_v2.pdf) are implemented in the `kinematic_model.py` python module, in the `forwardKinematics` function. 20 | 21 | ![forwardKinematics](../../images/forwardKinematics.png) 22 | 23 | ## Inverse kinematic calculations 24 | 25 | The [inverse kinematic calculations](Inverse_kinematics_EEZYbotARM.pdf) are implemented in the `kinematic_model.py` python module, in the `inverseKinematics` function. 26 | 27 | ![inverseKinematics](../../images/inverseKinematics.png) 28 | 29 | -------------------------------------------------------------------------------- /examples/example1_plot.py: -------------------------------------------------------------------------------- 1 | # Import EEZYbotARM library 2 | from easyEEZYbotARM.kinematic_model import EEZYbotARM_Mk2 3 | from easyEEZYbotARM.kinematic_model import EEZYbotARM_Mk1 4 | 5 | # initial joint angles 6 | jointAngle1 = 0 7 | jointAngle2 = 70 8 | jointAngle3 = -100 9 | 10 | # Create an EEZYbotARM Mk2 object 11 | myRobotArm = EEZYbotARM_Mk2(jointAngle1, jointAngle2, jointAngle3) 12 | # Plot it 13 | myRobotArm.plot() 14 | 15 | # Create an EEZYbotARM Mk1 object 16 | myRobotArm = EEZYbotARM_Mk1(jointAngle1, jointAngle2, jointAngle3) 17 | # Plot it 18 | myRobotArm.plot() 19 | -------------------------------------------------------------------------------- /examples/example2_forwardKinematics.py: -------------------------------------------------------------------------------- 1 | # Import EEZYbotARM library 2 | from easyEEZYbotARM.kinematic_model import EEZYbotARM_Mk2 3 | 4 | # Initialise robot arm with initial joint angles 5 | myRobotArm = EEZYbotARM_Mk2(initial_q1=0, initial_q2=70, initial_q3=-100) 6 | myRobotArm.plot() # plot it 7 | 8 | # Assign new joint angles 9 | a1 = 20 # joint angle 1 10 | a2 = 80 # joint angle 2 11 | a3 = -90 # joint angle 3 12 | 13 | # Compute forward kinematics 14 | x, y, z = myRobotArm.forwardKinematics(q1=a1, q2=a2, q3=a3) 15 | 16 | # Print the result 17 | print('With joint angles(degrees) q1={}, q2={}, q3={}, the cartesian position of the end effector(mm) is x={}, y={}, z={}'.format(a1, a2, a3, x, y, z)) 18 | 19 | # Visualise the new joint angles 20 | myRobotArm.updateJointAngles(q1=a1, q2=a2, q3=a3) 21 | myRobotArm.plot() 22 | -------------------------------------------------------------------------------- /examples/example3_inverseKinematics.py: -------------------------------------------------------------------------------- 1 | # Import EEZYbotARM library 2 | from easyEEZYbotARM.kinematic_model import EEZYbotARM_Mk2 3 | 4 | # Initialise robot arm with initial joint angles 5 | myRobotArm = EEZYbotARM_Mk2(initial_q1=0, initial_q2=70, initial_q3=-100) 6 | myRobotArm.plot() # plot it 7 | 8 | # Assign cartesian position where we want the robot arm end effector to move to 9 | # (x,y,z in mm from centre of robot base) 10 | x = 240 # mm 11 | y = 85 # mm 12 | z = 200 # mm 13 | 14 | # Compute inverse kinematics 15 | a1, a2, a3 = myRobotArm.inverseKinematics(x, y, z) 16 | 17 | # Print the result 18 | print('To move the end effector to the cartesian position (mm) x={}, y={}, z={}, the robot arm joint angles (degrees) are q1 = {}, q2= {}, q3 = {}'.format(x, y, z, a1, a2, a3)) 19 | 20 | # Visualise the new joint angles 21 | myRobotArm.updateJointAngles(q1=a1, q2=a2, q3=a3) 22 | myRobotArm.plot() 23 | -------------------------------------------------------------------------------- /examples/example4_make_the_EEZYbotARM_move.py: -------------------------------------------------------------------------------- 1 | # Please note for this example you must have an Arduino connected to the computer 2 | # with a PCA9685 (or equivalent) servo driver connected and powered. The Arduino sketch from 3 | # the 'arduino_sketches/easyEEZYbotARM_Arduino_Communication' folder must be uploaded 4 | # to the arduino. 5 | 6 | # This example shows the basis for moving the arduino servos. It does not link the 'kinematic model' together 7 | # with the Arduino. For an example of this please see example5_connect_kinematic_model_to_EEZYbotARM.py 8 | 9 | # The servos are plugged into the following pin positions on the PCA9685 board: 10 | # Servo_q1 -> EzzyBot base (q1) -> PCA9685 pin 1 11 | # Servo_q2 -> Main arm (q2) -> PCA9685 pin 2 12 | # Servo_q3 -> Horarm (q3) -> PCA9685 pin 3 13 | # Servo_EE -> End Effector (EE) -> PCA9685 pin 0 14 | 15 | from easyEEZYbotARM.serial_communication import arduinoController 16 | 17 | # Insert your Arduino serial port here 18 | myArduino = arduinoController(port="COM3") 19 | 20 | # Create some test data. 21 | # The angles used for this test data are 'raw' servo angles (i.e. not calibrated) 22 | # against the kinematic model for the EEZYbotARM. This example moves the EzzyBot base (q1) 23 | testData = [] 24 | testData.append(myArduino.composeMessage(servoAngle_q1=90, 25 | servoAngle_q2=90, 26 | servoAngle_q3=90, 27 | servoAngle_EE=10)) 28 | 29 | testData.append(myArduino.composeMessage(servoAngle_q1=90, 30 | servoAngle_q2=90, 31 | servoAngle_q3=90, 32 | servoAngle_EE=90)) 33 | 34 | 35 | # The connection should be managed in this sequence (will be simplified in future) 36 | # Open a serial port and connect to the Arduino 37 | myArduino.openSerialPort() 38 | # Send the test data which is managed by the 'run test' function' 39 | myArduino.communicate(data=testData, delay_between_commands=5) 40 | myArduino.closeSerialPort() # Close the serial port 41 | -------------------------------------------------------------------------------- /examples/example5_connect_kinematic_model_to_EEZYbotARM.py: -------------------------------------------------------------------------------- 1 | # Please note for this example you must have an Arduino connected to the computer 2 | # with a PCA9685 (or equivalent) servo driver connected and powered. The Arduino sketch from 3 | # the 'arduino_sketches/easyEEZYbotARM_Arduino_Communication' folder must be uploaded 4 | # to the arduino. 5 | 6 | # This example links the 'kinematic model' together with the Arduino controlling the 7 | # EEZYbotARM 8 | 9 | # You must have installed the servo's in their reference positions. If you are reading this and 10 | # you're not sure what this means, please email ben.money@gmail.com. The procedure for this step 11 | # will be published online at a later date. 12 | 13 | # The reference positions for the three servos are as follows: 14 | # - EzzyBot base (q1 -> 0 degrees) : 90 degree servo position is facing directly forwards 15 | # - Main arm (q2 -> 90 degrees): 90 degree servo position is with main arm perpendicular (at 90 degrees to) base 16 | # - Horarm (q3 -> -135): 90 degree servo poisition is with horarm servo link at 45 degrees to base 17 | 18 | # The servos are plugged into the following pin positions on the PCA9685 board: 19 | # Servo_q1 -> EzzyBot base (q1) -> PCA9685 pin 1 20 | # Servo_q2 -> Main arm (q2) -> PCA9685 pin 2 21 | # Servo_q3 -> Horarm (q3) -> PCA9685 pin 3 22 | # Servo_EE -> End Effector (EE) -> PCA9685 pin 0 23 | 24 | # Import EEZYbotARM library 25 | from easyEEZYbotARM.kinematic_model import EEZYbotARM_Mk2 26 | from easyEEZYbotARM.serial_communication import arduinoController 27 | 28 | # Insert your Arduino serial port here to initialise the arduino controller 29 | myArduino = arduinoController(port="COM3") 30 | myArduino.openSerialPort() 31 | 32 | # Initialise kinematic model with initial joint angles (home position) 33 | myVirtualRobotArm = EEZYbotARM_Mk2( 34 | initial_q1=0, initial_q2=90, initial_q3=-130) 35 | # Plot it 36 | myVirtualRobotArm.plot() 37 | 38 | # Define end effector open and closed angle 39 | servoAngle_EE_closed = 10 40 | servoAngle_EE_open = 90 41 | 42 | # Calculate the current servo angles 43 | servoAngle_q1, servoAngle_q2, servoAngle_q3 = myVirtualRobotArm.map_kinematicsToServoAngles() 44 | 45 | # Send the movement command to the arduino. The physical EEZYbotARM will move to this position 46 | myArduino.communicate(data=myArduino.composeMessage(servoAngle_q1=servoAngle_q1, 47 | servoAngle_q2=servoAngle_q2, 48 | servoAngle_q3=servoAngle_q3, 49 | servoAngle_EE=servoAngle_EE_open)) 50 | 51 | 52 | # Assign new cartesian position where we want the robot arm end effector to move to 53 | # (x,y,z in mm from centre of robot base) 54 | x = 240 # mm 55 | y = 85 # mm 56 | z = 200 # mm 57 | 58 | # Compute inverse kinematics 59 | a1, a2, a3 = myVirtualRobotArm.inverseKinematics(x, y, z) 60 | 61 | # Print the result 62 | print('To move the end effector to the cartesian position (mm) x={}, y={}, z={}, the robot arm joint angles (degrees) are q1 = {}, q2= {}, q3 = {}'.format(x, y, z, a1, a2, a3)) 63 | 64 | # Visualise the new joint angles 65 | myVirtualRobotArm.updateJointAngles(q1=a1, q2=a2, q3=a3) 66 | myVirtualRobotArm.plot() 67 | 68 | # Calculate the current servo angles 69 | servoAngle_q1, servoAngle_q2, servoAngle_q3 = myVirtualRobotArm.map_kinematicsToServoAngles() 70 | 71 | # Send the movement command to the arduino. The physical EEZYbotARM will move to this position 72 | myArduino.communicate(data=myArduino.composeMessage(servoAngle_q1=servoAngle_q1, 73 | servoAngle_q2=servoAngle_q2, 74 | servoAngle_q3=servoAngle_q3, 75 | servoAngle_EE=servoAngle_EE_open)) 76 | 77 | # Close the serial port 78 | myArduino.closeSerialPort() 79 | -------------------------------------------------------------------------------- /examples/example6.py: -------------------------------------------------------------------------------- 1 | # Example of forward and inverse kinematics with the EEZYbotARM_Mk1 2 | 3 | # Import EEZYbotARM library 4 | from easyEEZYbotARM.kinematic_model import EEZYbotARM_Mk1 5 | 6 | # Initialise robot arm with initial joint angles 7 | # Note it doesn't really matter too much what the initial joint angles are - they're only stored internally in the class. You don't have to use the intial values for anything if you don't want to! 8 | myRobotArm = EEZYbotARM_Mk1(initial_q1=0, initial_q2=70, initial_q3=-100) 9 | 10 | 11 | # --- FORWARD KINEMATICS EXAMPLE ---- 12 | 13 | # Assign new joint angles 14 | a1 = 0 # joint angle 1 15 | a2 = 90 # joint angle 2 16 | a3 = -130 # joint angle 3 17 | 18 | # Compute forward kinematics 19 | x, y, z = myRobotArm.forwardKinematics(q1=a1, q2=a2, q3=a3) 20 | 21 | # Print the result 22 | print('With joint angles(degrees) q1={}, q2={}, q3={}, the cartesian position of the end effector(mm) is x={}, y={}, z={}'.format(a1, a2, a3, x, y, z)) 23 | 24 | # Visualise the new joint angles 25 | myRobotArm.updateJointAngles(q1=a1, q2=a2, q3=a3) 26 | print("Here is the plot after competing forward kinematics with these joint angles, and this cartesian position") 27 | myRobotArm.plot() 28 | 29 | 30 | # --- INVERSE KINEMATICS EXAMPLE ---- 31 | 32 | # Assign cartesian position where we want the robot arm end effector to move to 33 | # (x,y,z in mm from centre of robot base) 34 | x = 120 # mm 35 | y = 0 # mm 36 | z = 90 # mm 37 | 38 | # Compute inverse kinematics 39 | a1, a2, a3 = myRobotArm.inverseKinematics(x, y, z) 40 | 41 | # Print the result 42 | print('To move the end effector to the cartesian position (mm) x={}, y={}, z={}, the robot arm joint angles (degrees) are q1 = {}, q2= {}, q3 = {}'.format(x, y, z, a1, a2, a3)) 43 | 44 | # Visualise the new joint angles 45 | myRobotArm.updateJointAngles(q1=a1, q2=a2, q3=a3) 46 | print("Here is the plot after competing inverse kinematics with these joint angles, and this cartesian position") 47 | myRobotArm.plot() 48 | -------------------------------------------------------------------------------- /fritzing_files/EEZYbotARM_PCA9685_v1.fzz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/fritzing_files/EEZYbotARM_PCA9685_v1.fzz -------------------------------------------------------------------------------- /fritzing_files/EEZYbotARM_PCA9685_v1_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/fritzing_files/EEZYbotARM_PCA9685_v1_bb.png -------------------------------------------------------------------------------- /images/forwardKinematics.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/images/forwardKinematics.png -------------------------------------------------------------------------------- /images/fritzingDiagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/images/fritzingDiagram.png -------------------------------------------------------------------------------- /images/introPicture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/images/introPicture.png -------------------------------------------------------------------------------- /images/inverseKinematics.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/images/inverseKinematics.png -------------------------------------------------------------------------------- /images/plottingAndWorkspace.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/images/plottingAndWorkspace.png -------------------------------------------------------------------------------- /images/servo_installation_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/images/servo_installation_position.png -------------------------------------------------------------------------------- /images/servo_one_calibration_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/images/servo_one_calibration_example.png -------------------------------------------------------------------------------- /images/softwareArchitecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/images/softwareArchitecture.png -------------------------------------------------------------------------------- /python_packages/easyEEZYbotARM/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/python_packages/easyEEZYbotARM/__init__.py -------------------------------------------------------------------------------- /python_packages/easyEEZYbotARM/kinematic_model.py: -------------------------------------------------------------------------------- 1 | 2 | #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 3 | # Program Title: kinematic_model.py 4 | # Program Purpose: Define EEZYbotARM_kinematics parent class and EEZYbotARM_Mk2 child class 5 | 6 | # **Version control** 7 | # v3.1 -> adding save of np array for complex hull, adding complex hull function (this works in very basic form) 8 | # v3.2 -> getting the complex hull function to pass data to a overall ploting function (it's also working) 9 | # v3.3 -> troubleshooting why the convex hull and workspace isn't quite right! [19 Nov 19] 10 | # v3.4 -> checking demo functionality 11 | # v3.5 -> moving to github 12 | # v3.6 -> adding MK1 version of the EEZYbotARM as a new sub-class 13 | # v3.7 -> squishing bug for Mk1 inverse kinematics ----> Solved :) ! 14 | 15 | #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 16 | 17 | # ------------------------------------------# 18 | # Imports # 19 | # ------------------------------------------# 20 | 21 | from math import * # for maths 22 | import matplotlib.pyplot as plt # for plotting 23 | from mpl_toolkits.mplot3d import Axes3D # for 3D plotting 24 | # [PUNCH LIST] NEED TO DO SOMETHING ABOUT THIS -> SAME AS ABOVE ! 25 | import mpl_toolkits.mplot3d as a3 26 | import numpy as np # for vector arithmetic 27 | import pathlib # for dealing with files 28 | # [PUNCH LIST] needed? -> it's only used to generate a random number, I should be able to delete (pending) 29 | import scipy as sp 30 | # [PUNCH LIST] needed for plotting the complex hull -> I think i can do this in another way! 31 | import pylab as pl 32 | from scipy.spatial import ConvexHull 33 | import pickle # used for saving python files 34 | 35 | 36 | # ------------------------------------------# 37 | # Helper functions # 38 | # ------------------------------------------# 39 | 40 | def plotCoOrd(T0toN, ax, lineColor): 41 | """ 42 | Plot a co-ordinate frame for a transformation matrix (in this case for a joint) using unit vectors. 43 | The plot is made using 3 'quiver objects' (arrows) 44 | 45 | --Parameters-- 46 | @T0toN -> the co-ordinate frame to plot 47 | @ax -> the matplotlib 3d axes object to make the plot on 48 | @lineColor -> the color the co-ordinate frame 49 | 50 | """ 51 | 52 | # Invert T0toN because we are rotating from the world frame to the N frame 53 | TNto0 = np.linalg.inv(T0toN) 54 | R = TNto0[0:3, 0:3] # Find rotation matrix 55 | T_XYZ = T0toN[0:3, 3] # Find translation vector 56 | 57 | # Unit Vector Positions before rotation for (x,y,x) positions and XYZ vectors (i) 58 | UV_XYZ = np.zeros((3, 3, 3), dtype=float) 59 | # Where dims are (start position of quiver, end position of quiver, respective xyz quivers) 60 | 61 | # define the unit vectors 62 | for i in range(0, 3): 63 | UV_XYZ[i, 1, i] = 50 64 | 65 | # define the co-ordinates to be plotted 66 | for i in range(0, 3): 67 | # make the rotation and translation 68 | UV_XYZ[:, 0, i] = R.dot(UV_XYZ[:, 0, i]) + T_XYZ[i] 69 | UV_XYZ[:, 1, i] = R.dot(UV_XYZ[:, 1, i]) + \ 70 | T_XYZ[i] # find end position (matlab) 71 | UV_XYZ[:, 2, i] = UV_XYZ[:, 1, i] - \ 72 | UV_XYZ[:, 0, i] # find end position (python) 73 | 74 | # these labels are used in the plot to label co-ordinate axes 75 | labels = ['X', 'Y', 'Z'] 76 | 77 | # plot the x, y, z quivers 78 | for i in range(0, 3): 79 | 80 | # data for quiver 81 | x = UV_XYZ[i, 0, 0] # label the co-ordinates 82 | y = UV_XYZ[i, 0, 1] 83 | z = UV_XYZ[i, 0, 2] 84 | u = UV_XYZ[i, 2, 0] 85 | v = UV_XYZ[i, 2, 1] 86 | w = UV_XYZ[i, 2, 2] 87 | 88 | # plot each quiver 89 | ax.quiver(x, y, z, u, v, w, length=80, normalize=True, 90 | color=lineColor, linewidth=0.5) 91 | 92 | # data for text 93 | labels = ['x', 'y', 'z'] 94 | zdir = None 95 | 96 | # plot text label 97 | ax.text(x+u, y+v, z+w, labels[i], zdir, color='gray') 98 | 99 | 100 | # These helper functions help set axes equal in 3d 101 | # Unaltered code from https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to 102 | def set_axes_radius(ax, origin, radius): 103 | ax.set_xlim3d([origin[0] - radius, origin[0] + radius]) 104 | ax.set_ylim3d([origin[1] - radius, origin[1] + radius]) 105 | ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) 106 | 107 | 108 | def set_axes_equal(ax): 109 | '''Make axes of 3D plot have equal scale so that spheres appear as spheres, 110 | cubes as cubes, etc.. This is one possible solution to Matplotlib's 111 | ax.set_aspect('equal') and ax.axis('equal') not working for 3D. 112 | 113 | Input 114 | ax: a matplotlib axis, e.g., as output from plt.gca(). 115 | ''' 116 | 117 | limits = np.array([ 118 | ax.get_xlim3d(), 119 | ax.get_ylim3d(), 120 | ax.get_zlim3d(), 121 | ]) 122 | 123 | origin = np.mean(limits, axis=1) 124 | radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0])) 125 | set_axes_radius(ax, origin, radius) 126 | 127 | 128 | def in_hull(p, hull): 129 | """ 130 | Test if points in `p` are in `hull` 131 | 132 | `p` should be a `NxK` coordinates of `N` points in `K` dimensions 133 | `hull` is either a scipy.spatial.Delaunay object or the `MxK` array of the 134 | coordinates of `M` points in `K`dimensions for which Delaunay triangulation 135 | will be computed 136 | 137 | 138 | --Example of use-- 139 | 140 | tested = np.random.rand(20,3) 141 | cloud = np.random.rand(50,3) 142 | 143 | print in_hull(tested,cloud) -> [True, True, False, etc.] 144 | 145 | """ 146 | from scipy.spatial import Delaunay 147 | if not isinstance(hull, Delaunay): 148 | hull = Delaunay(hull) 149 | 150 | return hull.find_simplex(p) >= 0 151 | 152 | 153 | # ------------------------------------------# 154 | # Parent class # 155 | # ------------------------------------------# 156 | 157 | class EEZYbotARM: 158 | """ 159 | --Description-- 160 | This is a parent class for the EExybot Robotic arm 161 | 162 | **Please note that you must always instantiate on a child class (either EEZYbotARMMk2() or EEZYbotARMMk1()) 163 | rather than instansiating the parent 'EEZYbotARM' class** 164 | 165 | --Methods-- 166 | Description of available methods in this class: 167 | - __init__ --> Initialise a robotic arm with current joint position 168 | - updateJointAngles --> Update the stored joint angles for the robot arm 169 | - checkErrorJointLimits --> Check if any of the supplied joint angles are outside of physical limits 170 | - FK_EEZYbotARM --> Perform forward kinematics to find the end effector position, given joint angles 171 | - IK_EEZYbotARM --> Perform inverse kinematics to find the joint angles, given end effector position 172 | - plot_EEZYbotARM --> Plot the robotic arm in 3D using matplotlib Axes 3D 173 | 174 | """ 175 | 176 | # Class attribute 177 | # e.g. species = 'mammal' 178 | 179 | # Initializer / Instance attributes 180 | def __init__(self, initial_q1, initial_q2, initial_q3): 181 | self.q1 = initial_q1 182 | self.q2 = initial_q2 183 | self.q3 = initial_q3 184 | 185 | # Instance methods 186 | def updateJointAngles(self, q1, q2, q3): 187 | """ 188 | --Description-- 189 | Update the three EEzybot Arm joint angles (by providing values in degrees) 190 | Will return an error message if the joint angles are outside of given limits 191 | 192 | --Optional **kwargs Parameters-- 193 | @q1 -> the value of the angle q1 (in degrees) 194 | @q2 -> the value of the angle q2 (in degrees) 195 | @q3 -> the value of the angle q3 (in degrees) 196 | 197 | --Returns-- 198 | Function doesn't return a value 199 | 200 | """ 201 | # Check given values are inside valid limits 202 | self.checkErrorJointLimits(q1=q1, q2=q2, q3=q3) 203 | 204 | # Assign new angles 205 | self.q1 = q1 206 | self.q2 = q2 207 | self.q3 = q3 208 | 209 | def checkErrorJointLimits(self, **kwargs): 210 | """ 211 | --Description-- 212 | Take the three angles for the EzzyBot Arm defined in kinematics. 213 | Use these to calculate whether the EzzyBot Arm is outside of joint limits 214 | If outside of joint limts return error code for appropriate joint 215 | Otherwise return zero value 216 | 217 | --Optional **kwargs Parameters-- 218 | @q1 -> the value of the angle q1 (in degrees) as used in the kinematics model 219 | @q2 -> the value of the angle q2 (in degrees) as used in the kinematics model 220 | @q3 -> the value of the angle q3 (in degrees) as used in the kinematics model 221 | 222 | --Returns-- 223 | servoAngle_q1, servoAngle_q2, servoAngle_q3 -> values in degrees for output to the physical servos 224 | 225 | """ 226 | # Use **kwargs if provided, otherwise use current values 227 | q1 = kwargs.get('q1', self.q1) 228 | q2 = kwargs.get('q2', self.q2) 229 | q3 = kwargs.get('q3', self.q3) 230 | 231 | # Find angle limits 232 | q3_min, q3_max = self.q3CalcLimits(q2=q2) 233 | 234 | # Check given angles are within limits, else raise exception 235 | if q1 < self.q1_min or q1 > self.q1_max: 236 | raise Exception('Value for q1 is outside joint limits. Joint limits in degrees are ({},{})'.format( 237 | self.q1_min, self.q1_max)) 238 | 239 | if q2 < self.q2_min or q2 > self.q2_max: 240 | raise Exception('Value for q2 is outside joint limits. Joint limits in degrees are ({},{})'.format( 241 | self.q2_min, self.q2_max)) 242 | 243 | if q3 < q3_min or q3 > q3_max: 244 | raise Exception( 245 | 'Value for q3 is outside joint limits. Joint limits in degrees are ({},{})'.format(q3_min, q3_max)) 246 | 247 | def forwardKinematics(self, **kwargs): 248 | """ 249 | --Description-- 250 | Given angles q1, q2, q3, find the forward kinematics for the end effector position of the EEZYbotARM 251 | 252 | If q1, q2, q3 are not provided then the current arm angles will be used (self.q1,q2,q3) 253 | 254 | The forward kinematics uses DH proximal convention and the method is disclosed seperate to this code 255 | 256 | The reference angle definitions are as follows: 257 | 258 | - EzzyBot base (q1) : 0 degree position is facing directly forwards 259 | - Main arm position (q2) 260 | - Horarm position(q3) 261 | 262 | --Optional **kwargs Parameters-- 263 | @q1 -> the value of the angle q1 (in degrees) as used in the kinematics model 264 | @q2 -> the value of the angle q2 (in degrees) as used in the kinematics model 265 | @q3 -> the value of the angle q3 (in degrees) as used in the kinematics model 266 | 267 | --Returns-- 268 | x_EE, y_EE, z_EE -> position of the end effector in the world frame (mm) 269 | 270 | """ 271 | 272 | # Use **kwargs if provided, otherwise use current values 273 | q1 = kwargs.get('q1', self.q1) 274 | q2 = kwargs.get('q2', self.q2) 275 | q3 = kwargs.get('q3', self.q3) 276 | 277 | # Convert to radians 278 | q1 = q1 * pi/180 279 | q2 = q2 * pi/180 280 | q3 = q3 * pi/180 281 | 282 | # Joint length definitions (for ease of reading code) 283 | L1 = self.L1 284 | L2 = self.L2 285 | L3 = self.L3 286 | L4 = self.L4 287 | 288 | # Find the position of the end effector using the forward kinematics equations 289 | x_EE = round((cos(q1) * (cos(q2+q3)*L3 + cos(q2)*L2))+(L4*cos(q1)), 3) 290 | y_EE = round((sin(q1) * (cos(q2+q3)*L3 + cos(q2)*L2))+(L4*sin(q1)), 3) 291 | z_EE = round((L1 + sin(q2)*L2 + sin(q2+q3)*L3), 3) 292 | 293 | # Return the end effector position in (mm) 294 | return x_EE, y_EE, z_EE 295 | 296 | def inverseKinematics(self, x_EE, y_EE, z_EE): 297 | """ 298 | --Description-- 299 | Function: to find the Inverse Kinematics for the EEZYbotARM Mk2 300 | 301 | Description: given x y z positions of the desired EE position inside the 302 | manipulator workspace, find the corresponding joint angles 303 | 304 | --Parameters-- 305 | x_EE, y_EE, z_EE -> the cartesian position of the end effector in the world frame 306 | 307 | --Returns-- 308 | q1, q2, q3 -> Corresponding joint angles in degrees 309 | 310 | """ 311 | # DH parameters (Proximal Convention) 312 | L1 = self.L1 313 | L2 = self.L2 314 | L3 = self.L3 315 | L4 = self.L4 316 | 317 | # Find the value for the fist angle 318 | q1 = atan2(y_EE, x_EE) 319 | 320 | # Find the values for the position of joint #4 for x, y, z 321 | x_4 = x_EE - (L4 * cos(q1)) 322 | y_4 = y_EE - (L4 * sin(q1)) 323 | z_4 = z_EE 324 | 325 | # Find the length of the third side of the (virtual) triangle made by L2, 326 | # L3 in the vertical plane of the robot arm 327 | 328 | # --- Specify the z poisition of joint #1 329 | z_1 = L1 330 | 331 | # --- First we find the z distance between joint #1 and joint #4 (in the world frame) 332 | z_1_4 = z_4 - z_1 333 | 334 | # --- Find the true distance (in x, y plane) to joint #4 335 | xy_4 = sqrt((x_4**2)+(y_4**2)) 336 | 337 | # --- Then we find the length of the virtual side made by the right angle triangle 338 | v_side = sqrt((z_1_4**2) + (xy_4**2)) 339 | 340 | # Find the value for the angle at joint #3 341 | q3 = - (pi - acos((L2**2 + L3**2 - v_side**2)/(2 * L2 * L3))) 342 | 343 | # Find the value for the angle at joint #2 %DEBUG HERE 344 | # --- Find the value of the angle from the x-y plane to the virtual side 345 | q2_a = atan2(z_1_4, xy_4) 346 | 347 | q2_b = acos((v_side**2 + L2**2 - L3**2)/(2 * v_side * L2)) 348 | 349 | q2 = q2_a + q2_b # NOTE there's some more work to do here to make this correctly summation or subtraction dependant on the position of the arm! 350 | 351 | # Print the input world frame position of the end effector 352 | print('Input Position of End Effector: \n') 353 | print('x_EE: {}'.format(x_EE)) 354 | print('y_EE: {}'.format(y_EE)) 355 | print('z_EE: {} \n'.format(z_EE)) 356 | 357 | # Print the output joint angles 358 | print('Ouput joint angles: \n') 359 | print('q1: {:+.2f}'.format(q1 * 180/pi)) 360 | print('q2: {:+.2f}'.format(q2 * 180/pi)) 361 | print('q3: {:+.2f} \n'.format(q3 * 180/pi)) 362 | 363 | # round values 364 | q1 = round(q1 * 180/pi, 2) 365 | q2 = round(q2 * 180/pi, 2) 366 | q3 = round(q3 * 180/pi, 2) 367 | 368 | return q1, q2, q3 369 | 370 | def plot(self, **kwargs): 371 | """ 372 | --Description-- 373 | Given angles q1, q2, q3, plot EEZybot arm and co-ordinate frames for all joints using matplotlib 374 | Function computes the configuration of the manipulator and plots it using DH proximal convention. 375 | 376 | The function will raise an exception if any of the angles are outside defined limits 377 | 378 | The reference angle definitions are as follows: 379 | 380 | - EzzyBot base (q1) : 0 degree position is facing directly forwards 381 | - Main arm position (q2) 382 | - Horarm position(q3) 383 | 384 | --Required-- 385 | Helper functions required (included in the EEzybot code) 386 | - plotCoOrd() 387 | - set_axes_radius() 388 | - set_axes_equal() 389 | 390 | --Optional **kwargs parameters-- 391 | @q1 -> the value of the angle q1 (in degrees) as used in the kinematics model 392 | @q2 -> the value of the angle q2 (in degrees) as used in the kinematics model 393 | @q3 -> the value of the angle q3 (in degrees) as used in the kinematics model 394 | 395 | --Returns-- 396 | fig, ax -> matplotlib figure and axes objects showing position of all actuator joints and links in world frame 397 | 398 | """ 399 | 400 | # Use **kwargs if provided, otherwise use current values 401 | q1 = kwargs.get('q1', self.q1) 402 | q2 = kwargs.get('q2', self.q2) 403 | q3 = kwargs.get('q3', self.q3) 404 | 405 | # Convert angles to radians 406 | q1 = q1 * pi/180 407 | q2 = q2 * pi/180 408 | q3 = q3 * pi/180 409 | 410 | # DH parameters (Proximal Convention), 411 | L1 = self.L1 412 | L2 = self.L2 413 | L3 = self.L3 414 | L4 = self.L4 415 | # Lengths for the links which attach to the hoarm 416 | L2A = self.L2A 417 | LAB = self.LAB 418 | LB3 = self.LB3 419 | 420 | # DH table 421 | DH = np.array([[0, 0, L1, q1], 422 | [0, pi/2, 0, q2], 423 | [L2, 0, 0, q3], 424 | [L3, 0, 0, 0], 425 | [0, -pi/2, 0, 0]]) 426 | 427 | # Find number of rows in DH table 428 | rows, cols = DH.shape 429 | 430 | # Pre-allocate Array to store Transformation matrix 431 | T = np.zeros((4, 4, rows), dtype=float) 432 | 433 | # Determine transformation matrix between each frame 434 | for i in range(rows): 435 | T[:, :, i] = [[cos(DH[i, 3]), -sin(DH[i, 3]), 0, DH[i, 0]], 436 | [sin(DH[i, 3])*cos(DH[i, 1]), cos(DH[i, 3]) * 437 | cos(DH[i, 1]), -sin(DH[i, 1]), -sin(DH[i, 1])*DH[i, 2]], 438 | [sin(DH[i, 3])*sin(DH[i, 1]), cos(DH[i, 3]) * 439 | sin(DH[i, 1]), cos(DH[i, 1]), cos(DH[i, 1])*DH[i, 2]], 440 | [0, 0, 0, 1]] 441 | 442 | # Create the transformation frames with repect to the world frame (the base of the EEzybot arm) 443 | 444 | # --- Calculate Transformation matrix to each frame wrt the base. (matrix dot multiplication) 445 | T00 = np.identity(4) 446 | T01 = T[:, :, 0] 447 | T02 = T[:, :, 0].dot(T[:, :, 1]) 448 | T03 = T[:, :, 0].dot(T[:, :, 1]).dot(T[:, :, 2]) 449 | T04 = T[:, :, 0].dot(T[:, :, 1]).dot(T[:, :, 2]).dot(T[:, :, 3]) 450 | 451 | # --- Create frame 5 (note this is just a rotation of frame T04) 452 | R5 = T04[0:3, 0:3] # Find rotation matrix 453 | T45 = np.zeros((4, 4)) 454 | T45[0:3, 0:3] = np.linalg.inv(R5) 455 | T45[3, 3] = 1 # Create transformation matrix from frame 4 to frame 5 456 | 457 | # Create the transformation matrix from the world frame to frame 5 (without z rotation) 458 | T05 = T04.dot(T45) 459 | 460 | # --- Construct a transformation matrix to make the Z rotation of T05 by magnitude q1 461 | TZRot = np.array([[cos(q1), -sin(q1), 0, 0], 462 | [sin(q1), cos(q1), 0, 0], 463 | [0, 0, 1, 0], 464 | [0, 0, 0, 1]]) 465 | 466 | # Create the transformation matrix from the world frame to frame 5 (with z rotation) 467 | T05_true = T05.dot(TZRot) 468 | 469 | # -- Create Frame EE (Do the same for the end effector frame) 470 | T5EE = np.array([[1, 0, 0, L4 * cos(q1)], 471 | [0, 1, 0, L4 * sin(q1)], 472 | [0, 0, 1, 0], 473 | [0, 0, 0, 1]]) 474 | 475 | TEE = T05.dot(T5EE).dot(TZRot) # translate and rotate ! 476 | 477 | # --- Create the frames for the links which attach to the hoarm 478 | q3_a = pi - (- q3) # adjusted q3 value 479 | 480 | # --- --- For Frame A 481 | 482 | T2A_rot = np.array([[cos(q3_a), -sin(q3_a), 0, 0], # Rotation about z axis by q2 483 | [sin(q3_a), cos(q3_a), 0, 0], 484 | [0, 0, 1, 0], 485 | [0, 0, 0, 1]]) 486 | 487 | T2A_trans = np.array([[1, 0, 0, L2A], # Translation along x axis by distance L2A 488 | [0, 1, 0, 0], 489 | [0, 0, 1, 0], 490 | [0, 0, 0, 1]]) 491 | 492 | T2A = T2A_rot.dot(T2A_trans) 493 | 494 | T0A = T02.dot(T2A) # Frame for co-ordinate A 495 | 496 | # --- --- For Frame B 497 | 498 | TAB_rot = np.array([[cos(-(q3_a)), -sin(-(q3_a)), 0, 0], # Rotation about z axis by -(180-q2) 499 | [sin(-(q3_a)), cos(-(q3_a)), 0, 0], 500 | [0, 0, 1, 0], 501 | [0, 0, 0, 1]]) 502 | 503 | TAB_trans = np.array([[1, 0, 0, LAB], # Translation along x axis by distance L2A 504 | [0, 1, 0, 0], 505 | [0, 0, 1, 0], 506 | [0, 0, 0, 1]]) 507 | 508 | TAB = TAB_rot.dot(TAB_trans) 509 | 510 | T0B = T0A.dot(TAB) # Frame for co-ordinate A 511 | 512 | # Defines the position of each joint. 513 | # Joint 1 (This acts as the base) 514 | Base = np.array([0, 0, 0]) 515 | J2_Pos = np.array([T01[0, 3], T01[1, 3], T01[2, 3]]) # Joint 2 516 | J3_Pos = np.array([T02[0, 3], T02[1, 3], T02[2, 3]]) # Joint 3 517 | # Joint 4. it is assumed that the origin of joint 4 and 5 coincide. 518 | J4_Pos = np.array([T03[0, 3], T03[1, 3], T03[2, 3]]) 519 | J5_Pos = np.array([T04[0, 3], T04[1, 3], T04[2, 3]]) 520 | EE_Pos = np.array([TEE[0, 3], TEE[1, 3], TEE[2, 3]]) 521 | # --- Horarm joints 522 | A_Pos = np.array([T0A[0, 3], T0A[1, 3], T0A[2, 3]]) 523 | B_Pos = np.array([T0B[0, 3], T0B[1, 3], T0B[2, 3]]) 524 | 525 | # Coordinates for each link. 526 | Link1 = list(zip(Base, J2_Pos)) # From base to Joint 2 527 | Link2 = list(zip(J2_Pos, J3_Pos)) # From Joint 2 to Joint 3 528 | Link3 = list(zip(J3_Pos, J4_Pos)) # From Joint 3 to Joint 4 529 | Link4 = list(zip(J4_Pos, J5_Pos)) # From Joint 4 to Joint 5 530 | Link5 = list(zip(J5_Pos, EE_Pos)) # From Joint 5 to the end effector 531 | # --- Horarm links 532 | LinkA = list(zip(J2_Pos, A_Pos)) # From Joint 2 to Joint A 533 | LinkB = list(zip(A_Pos, B_Pos)) # From Joint A to Joint B 534 | LinkC = list(zip(B_Pos, J4_Pos)) # From Joint B to Joint 4 535 | 536 | # create the figure 537 | fig = plt.figure() 538 | 539 | # add an axis 540 | ax = fig.add_subplot(111, projection='3d') 541 | 542 | # add labels to the plot 543 | ax.set(title="3-d simulation of EEZYbotARM", xlabel="x (mm)", 544 | ylabel="y (mm)", zlabel="z (mm)") 545 | 546 | # set axis equal 547 | # ax.set_aspect('equal') # important! 548 | 549 | # data for lines 550 | linewidth = 3 551 | 552 | # plot the lines 553 | ax.plot(Link1[0], Link1[1], Link1[2], 554 | label="Link1", linewidth=linewidth) 555 | #ax.plot(Link2[0],Link2[1], Link2[2], label="Link2", linewidth=linewidth) 556 | ax.plot(Link3[0], Link3[1], Link3[2], 557 | label="Link2", linewidth=linewidth) 558 | ax.plot(Link4[0], Link4[1], Link4[2], 559 | label="Link3", linewidth=linewidth) 560 | ax.plot(Link5[0], Link5[1], Link5[2], 561 | label="Link4", linewidth=linewidth) 562 | 563 | # --- plot the lines for the horarm links 564 | ax.plot(LinkA[0], LinkA[1], LinkA[2], 565 | linewidth=linewidth, color='lightgrey') 566 | ax.plot(LinkB[0], LinkB[1], LinkB[2], 567 | linewidth=linewidth, color='lightgrey') 568 | ax.plot(LinkC[0], LinkC[1], LinkC[2], 569 | linewidth=linewidth, color='lightgrey') 570 | 571 | # add a legend 572 | ax.legend() 573 | 574 | # plot co-ordinate frames 575 | plotCoOrd(T00, ax, lineColor='blue') 576 | plotCoOrd(T02, ax, lineColor='orange') 577 | plotCoOrd(T03, ax, lineColor='green') 578 | plotCoOrd(T05_true, ax, lineColor='red') 579 | plotCoOrd(TEE, ax, lineColor='grey') 580 | 581 | # plotCoOrd(T0A, ax, lineColor='lightgrey') # uncommenting will show servo 3 linkage co-ordinate frames 582 | # plotCoOrd(T0B, ax, lineColor='lightgrey') 583 | 584 | # show the plot 585 | # important code such that all axes display with equal scaling in 3d! 586 | set_axes_equal(ax) 587 | plt.show() 588 | 589 | # define the position of the end effector 590 | x_EE = round(float(TEE[0, 3]), 3) 591 | y_EE = round(float(TEE[1, 3]), 3) 592 | z_EE = round(float(TEE[2, 3]), 3) 593 | 594 | # Debug step 595 | print("plot_EEZYbotARM function --> End effector position (mm) is x: {}, y:{}, z:{}".format(x_EE, y_EE, z_EE)) 596 | 597 | return fig, ax 598 | 599 | def plotWithHull(self, **kwargs): 600 | """ 601 | --Description-- [TEST FOR PLOTTING WITH HULL!] 602 | Given angles q1, q2, q3, plot EEZybot arm and co-ordinate frames for all joints using matplotlib 603 | Function computes the configuration of the manipulator and plots it using DH proximal convention. 604 | 605 | The function will raise an exception if any of the angles are outside defined limits 606 | 607 | The reference angle definitions are as follows: 608 | 609 | - EzzyBot base (q1) : 0 degree position is facing directly forwards 610 | - Main arm position (q2) 611 | - Horarm position(q3) 612 | 613 | --Required-- 614 | Helper functions required (included in the EEzybot code) 615 | - plotCoOrd() 616 | - set_axes_radius() 617 | - set_axes_equal() 618 | 619 | --Optional **kwargs parameters-- 620 | @q1 -> the value of the angle q1 (in degrees) as used in the kinematics model 621 | @q2 -> the value of the angle q2 (in degrees) as used in the kinematics model 622 | @q3 -> the value of the angle q3 (in degrees) as used in the kinematics model 623 | 624 | --Returns-- 625 | fig, ax -> matplotlib figure and axes objects showing position of all actuator joints and links in world frame 626 | 627 | """ 628 | 629 | # Use **kwargs if provided, otherwise use current values 630 | q1 = kwargs.get('q1', self.q1) 631 | q2 = kwargs.get('q2', self.q2) 632 | q3 = kwargs.get('q3', self.q3) 633 | 634 | # Convert angles to radians 635 | q1 = q1 * pi/180 636 | q2 = q2 * pi/180 637 | q3 = q3 * pi/180 638 | 639 | # DH parameters (Proximal Convention), 640 | L1 = self.L1 641 | L2 = self.L2 642 | L3 = self.L3 643 | L4 = self.L4 644 | # Lengths for the links which attach to the hoarm 645 | L2A = self.L2A 646 | LAB = self.LAB 647 | LB3 = self.LB3 648 | 649 | # DH table 650 | DH = np.array([[0, 0, L1, q1], 651 | [0, pi/2, 0, q2], 652 | [L2, 0, 0, q3], 653 | [L3, 0, 0, 0], 654 | [0, -pi/2, 0, 0]]) 655 | 656 | # Find number of rows in DH table 657 | rows, cols = DH.shape 658 | 659 | # Pre-allocate Array to store Transformation matrix 660 | T = np.zeros((4, 4, rows), dtype=float) 661 | 662 | # Determine transformation matrix between each frame 663 | for i in range(rows): 664 | T[:, :, i] = [[cos(DH[i, 3]), -sin(DH[i, 3]), 0, DH[i, 0]], 665 | [sin(DH[i, 3])*cos(DH[i, 1]), cos(DH[i, 3]) * 666 | cos(DH[i, 1]), -sin(DH[i, 1]), -sin(DH[i, 1])*DH[i, 2]], 667 | [sin(DH[i, 3])*sin(DH[i, 1]), cos(DH[i, 3]) * 668 | sin(DH[i, 1]), cos(DH[i, 1]), cos(DH[i, 1])*DH[i, 2]], 669 | [0, 0, 0, 1]] 670 | 671 | # Create the transformation frames with repect to the world frame (the base of the EEzybot arm) 672 | 673 | # --- Calculate Transformation matrix to each frame wrt the base. (matrix dot multiplication) 674 | T00 = np.identity(4) 675 | T01 = T[:, :, 0] 676 | T02 = T[:, :, 0].dot(T[:, :, 1]) 677 | T03 = T[:, :, 0].dot(T[:, :, 1]).dot(T[:, :, 2]) 678 | T04 = T[:, :, 0].dot(T[:, :, 1]).dot(T[:, :, 2]).dot(T[:, :, 3]) 679 | 680 | # --- Create frame 5 (note this is just a rotation of frame T04) 681 | R5 = T04[0:3, 0:3] # Find rotation matrix 682 | T45 = np.zeros((4, 4)) 683 | T45[0:3, 0:3] = np.linalg.inv(R5) 684 | T45[3, 3] = 1 # Create transformation matrix from frame 4 to frame 5 685 | 686 | # Create the transformation matrix from the world frame to frame 5 (without z rotation) 687 | T05 = T04.dot(T45) 688 | 689 | # --- Construct a transformation matrix to make the Z rotation of T05 by magnitude q1 690 | TZRot = np.array([[cos(q1), -sin(q1), 0, 0], 691 | [sin(q1), cos(q1), 0, 0], 692 | [0, 0, 1, 0], 693 | [0, 0, 0, 1]]) 694 | 695 | # Create the transformation matrix from the world frame to frame 5 (with z rotation) 696 | T05_true = T05.dot(TZRot) 697 | 698 | # -- Create Frame EE (Do the same for the end effector frame) 699 | T5EE = np.array([[1, 0, 0, L4 * cos(q1)], 700 | [0, 1, 0, L4 * sin(q1)], 701 | [0, 0, 1, 0], 702 | [0, 0, 0, 1]]) 703 | 704 | TEE = T05.dot(T5EE).dot(TZRot) # translate and rotate ! 705 | 706 | # --- Create the frames for the links which attach to the hoarm 707 | q3_a = pi - (- q3) # adjusted q3 value 708 | 709 | # --- --- For Frame A 710 | 711 | T2A_rot = np.array([[cos(q3_a), -sin(q3_a), 0, 0], # Rotation about z axis by q2 712 | [sin(q3_a), cos(q3_a), 0, 0], 713 | [0, 0, 1, 0], 714 | [0, 0, 0, 1]]) 715 | 716 | T2A_trans = np.array([[1, 0, 0, L2A], # Translation along x axis by distance L2A 717 | [0, 1, 0, 0], 718 | [0, 0, 1, 0], 719 | [0, 0, 0, 1]]) 720 | 721 | T2A = T2A_rot.dot(T2A_trans) 722 | 723 | T0A = T02.dot(T2A) # Frame for co-ordinate A 724 | 725 | # --- --- For Frame B 726 | 727 | TAB_rot = np.array([[cos(-(q3_a)), -sin(-(q3_a)), 0, 0], # Rotation about z axis by -(180-q2) 728 | [sin(-(q3_a)), cos(-(q3_a)), 0, 0], 729 | [0, 0, 1, 0], 730 | [0, 0, 0, 1]]) 731 | 732 | TAB_trans = np.array([[1, 0, 0, LAB], # Translation along x axis by distance L2A 733 | [0, 1, 0, 0], 734 | [0, 0, 1, 0], 735 | [0, 0, 0, 1]]) 736 | 737 | TAB = TAB_rot.dot(TAB_trans) 738 | 739 | T0B = T0A.dot(TAB) # Frame for co-ordinate A 740 | 741 | # Defines the position of each joint. 742 | # Joint 1 (This acts as the base) 743 | Base = np.array([0, 0, 0]) 744 | J2_Pos = np.array([T01[0, 3], T01[1, 3], T01[2, 3]]) # Joint 2 745 | J3_Pos = np.array([T02[0, 3], T02[1, 3], T02[2, 3]]) # Joint 3 746 | # Joint 4. it is assumed that the origin of joint 4 and 5 coincide. 747 | J4_Pos = np.array([T03[0, 3], T03[1, 3], T03[2, 3]]) 748 | J5_Pos = np.array([T04[0, 3], T04[1, 3], T04[2, 3]]) 749 | EE_Pos = np.array([TEE[0, 3], TEE[1, 3], TEE[2, 3]]) 750 | # --- Horarm joints 751 | A_Pos = np.array([T0A[0, 3], T0A[1, 3], T0A[2, 3]]) 752 | B_Pos = np.array([T0B[0, 3], T0B[1, 3], T0B[2, 3]]) 753 | 754 | # Coordinates for each link. 755 | Link1 = list(zip(Base, J2_Pos)) # From base to Joint 2 756 | Link2 = list(zip(J2_Pos, J3_Pos)) # From Joint 2 to Joint 3 757 | Link3 = list(zip(J3_Pos, J4_Pos)) # From Joint 3 to Joint 4 758 | Link4 = list(zip(J4_Pos, J5_Pos)) # From Joint 4 to Joint 5 759 | Link5 = list(zip(J5_Pos, EE_Pos)) # From Joint 5 to the end effector 760 | # --- Horarm links 761 | LinkA = list(zip(J2_Pos, A_Pos)) # From Joint 2 to Joint A 762 | LinkB = list(zip(A_Pos, B_Pos)) # From Joint A to Joint B 763 | LinkC = list(zip(B_Pos, J4_Pos)) # From Joint B to Joint 4 764 | 765 | # create the figure 766 | fig = plt.figure() 767 | 768 | # add an axis 769 | ax = fig.add_subplot(111, projection='3d') 770 | 771 | # add labels to the plot 772 | ax.set(title="3-d plot for EEZYbotARM Mk2 manipulator in XYZ world frame", xlabel="x (mm)", 773 | ylabel="y (mm)", zlabel="z (mm)") 774 | 775 | # set axis equal 776 | ax.set_aspect('equal') # important! 777 | 778 | # Set limits for plot 779 | # ax.set_xlim([-350, 350]) 780 | # ax.set_ylim([-350, 350]) 781 | # ax.set_zlim([-350, 350]) 782 | # data for lines 783 | linewidth = 3 784 | 785 | # plot the lines 786 | ax.plot(Link1[0], Link1[1], Link1[2], 787 | label="Link1", linewidth=linewidth) 788 | #ax.plot(Link2[0],Link2[1], Link2[2], label="Link2", linewidth=linewidth) 789 | ax.plot(Link3[0], Link3[1], Link3[2], 790 | label="Link2", linewidth=linewidth) 791 | ax.plot(Link4[0], Link4[1], Link4[2], 792 | label="Link3", linewidth=linewidth) 793 | ax.plot(Link5[0], Link5[1], Link5[2], 794 | label="Link4", linewidth=linewidth) 795 | 796 | # --- plot the lines for the horarm links 797 | ax.plot(LinkA[0], LinkA[1], LinkA[2], 798 | linewidth=linewidth, color='lightgrey') 799 | ax.plot(LinkB[0], LinkB[1], LinkB[2], 800 | linewidth=linewidth, color='lightgrey') 801 | ax.plot(LinkC[0], LinkC[1], LinkC[2], 802 | linewidth=linewidth, color='lightgrey') 803 | 804 | # add a legend 805 | ax.legend() 806 | 807 | # plot co-ordinate frames 808 | plotCoOrd(T00, ax, lineColor='blue') 809 | plotCoOrd(T02, ax, lineColor='orange') 810 | plotCoOrd(T03, ax, lineColor='green') 811 | plotCoOrd(T05_true, ax, lineColor='red') 812 | plotCoOrd(TEE, ax, lineColor='black') 813 | 814 | # plotCoOrd(T0A, ax, lineColor='lightgrey') # uncommenting will show servo 3 linkage co-ordinate frames 815 | # plotCoOrd(T0B, ax, lineColor='lightgrey') 816 | 817 | # ---Add the plot of the complex hull--- 818 | 819 | # Define directory to load the convex hull 820 | self.directory = '/data' 821 | self.convexHullFilename = '/EEZYbotARM_workspace/workspace_convexHull.p' 822 | 823 | # Load the convex hull 824 | with open(self.directory+self.convexHullFilename, 'rb') as fp: 825 | simplical_facet_corners = pickle.load(fp) 826 | 827 | facetCol = [0.5, 0.8, 0.4, 0.3] 828 | lineCol = [0.8, 0.8, 0.8, 0.2] 829 | 830 | # Plot surface traingulation of the convex hull 831 | for simplical_facet_corner in simplical_facet_corners: 832 | vtx = simplical_facet_corner 833 | tri = a3.art3d.Poly3DCollection([vtx], linewidths=1, alpha=0.3) 834 | tri.set_color(facetCol) 835 | tri.set_edgecolor(lineCol) 836 | ax.add_collection3d(tri) 837 | 838 | # show the plot 839 | # important code such that all axes display with equal scaling in 3d! 840 | set_axes_equal(ax) 841 | # plt.show() 842 | 843 | # define the position of the end effector 844 | x_EE = round(float(TEE[0, 3]), 3) 845 | y_EE = round(float(TEE[1, 3]), 3) 846 | z_EE = round(float(TEE[2, 3]), 3) 847 | 848 | # Debug step 849 | print("plot_EEZYbotARM function --> End effector position (mm) is x: {}, y:{}, z:{}".format(x_EE, y_EE, z_EE)) 850 | 851 | return fig, ax 852 | 853 | def plot_EEZYbotARM_workspace(self, stepSize=20, **kwargs): 854 | """ 855 | --Description-- 856 | Given angles q1, q2, q3 857 | 858 | --Required-- 859 | Helper functions required (included in the EEzybot code) 860 | - plotCoOrd() 861 | - set_axes_radius() 862 | - set_axes_equal() 863 | 864 | --Optional **kwargs parameters-- 865 | @q1 -> the value of the angle q1 (in degrees) as used in the kinematics model 866 | @q2 -> the value of the angle q2 (in degrees) as used in the kinematics model 867 | @q3 -> the value of the angle q3 (in degrees) as used in the kinematics model 868 | 869 | --Returns-- 870 | fig -> matplotlib figure showing position of all actuator joints and links in world frame 871 | x_EE, y_EE, z_EE -> position of the end effector in the world frame (mm) 872 | 873 | """ 874 | # DH parameters (Proximal Convention), 875 | L1 = self.L1 876 | L2 = self.L2 877 | L3 = self.L3 878 | L4 = self.L4 879 | 880 | # Angle limits 881 | q1_min = self.q1_min * pi/180 # radians 882 | q1_max = self.q1_max * pi/180 883 | q1_numSteps = stepSize 884 | 885 | q2_min = self.q2_min * pi/180 886 | q2_max = self.q2_max * pi/180 887 | q2_numSteps = stepSize 888 | 889 | q3_numSteps = stepSize # max, min not yet known due to dependance on q2 890 | 891 | # Step sizes 892 | q1_stepSize = (q1_max-q1_min)/q1_numSteps 893 | q2_stepSize = (q2_max-q2_min)/q2_numSteps 894 | 895 | # Size of pre-allocation for array 896 | arraySize = int((q1_numSteps+1) * (q2_numSteps+1) * (q3_numSteps+1)) 897 | 898 | # Pre-allocation of arrays for results 899 | xEndEffectorPoints = np.zeros(arraySize, dtype=float) 900 | yEndEffectorPoints = np.zeros(arraySize, dtype=float) 901 | zEndEffectorPoints = np.zeros(arraySize, dtype=float) 902 | 903 | # Array index 904 | index = 0 905 | 906 | # set intial variable value 907 | q1 = q1_min 908 | 909 | # Loop through q1 angles 910 | for i in range(0, q1_numSteps+1): 911 | # for i in range(0, 1): 912 | 913 | print("Progress -> {} / {}".format(i, q1_numSteps)) 914 | # print("q1 = ", q1*180/pi) 915 | 916 | # set inital q2 value 917 | q2 = q2_min 918 | 919 | # increment index 920 | i += 1 921 | 922 | # Loop through q2 angles 923 | for j in range(0, q2_numSteps+1): 924 | 925 | # print("q2 = ", q2 *180/pi) 926 | # print("q2 stepsize= ", q2_stepSize*180/pi) 927 | 928 | # calculate q3 limits for this q2 angle 929 | temp = (q2*180/pi) 930 | q3_min, q3_max = self.q3CalcLimits(q2=temp) 931 | 932 | # convert to radians 933 | q3_min = q3_min * pi/180 934 | q3_max = q3_max * pi/180 935 | # set intial variable value 936 | q3 = q3_min 937 | # calculate q3 stepsize 938 | q3_stepSize = (q3_max-q3_min)/q3_numSteps 939 | 940 | # increment index 941 | j += 1 942 | 943 | # Loop through q3 angles 944 | for k in range(0, q3_numSteps+1): 945 | 946 | # print("q3 = ", q3*180/pi) 947 | 948 | # Calculate end effector x, y, z position and store it 949 | xEndEffectorPoints[index] = round( 950 | (cos(q1) * (cos(q2+q3)*L3 + cos(q2)*L2))+(L4*cos(q1)), 1) 951 | yEndEffectorPoints[index] = round( 952 | (sin(q1) * (cos(q2+q3)*L3 + cos(q2)*L2))+(L4*sin(q1)), 1) 953 | zEndEffectorPoints[index] = round( 954 | (L1 + sin(q2)*L2 + sin(q2+q3)*L3), 1) 955 | 956 | # increment loop index 957 | k += 1 958 | # increment array index 959 | index += 1 960 | # increase q3 value by stepsize 961 | q3 = q3 + q3_stepSize 962 | 963 | # increase q2 value by stepsize 964 | q2 = q2 + q2_stepSize 965 | 966 | # increase q1 value by stepsize 967 | q1 = q1 + q1_stepSize 968 | 969 | # create the figure 970 | fig_workspace = plt.figure() 971 | 972 | # add an axis 973 | ax_workspace = fig_workspace.add_subplot(111, projection='3d') 974 | 975 | # add labels to the plot 976 | ax_workspace.set(title="3-d plot for EEZYbotARM Mk2 workspace in XYZ world frame", xlabel="x (mm)", 977 | ylabel="y (mm)", zlabel="z (mm)") 978 | 979 | # plot the data 980 | ax_workspace.scatter( 981 | xEndEffectorPoints[0:index], yEndEffectorPoints[0:index], zEndEffectorPoints[0:index], c='red') 982 | 983 | # define the world frame 984 | T00 = np.identity(4) 985 | 986 | # plot co-ordinate frames 987 | plotCoOrd(T00, ax_workspace, lineColor='blue') 988 | 989 | # show the plot 990 | # important code such that all axes display with equal scaling in 3d! 991 | set_axes_equal(ax_workspace) 992 | # plt.show() 993 | 994 | # Code added in v3.1 to save the end effector points array 995 | # Append the sampled points so that they are in the same numpy array 996 | endEffectorPointsXYZ = xEndEffectorPoints[0:index].copy() 997 | endEffectorPointsXYZ = np.expand_dims( 998 | endEffectorPointsXYZ, axis=1) # Equivalent to x[:,np.newaxis] 999 | endEffectorPointsXYZ = np.append(endEffectorPointsXYZ, np.expand_dims( 1000 | yEndEffectorPoints[0:index], axis=1), axis=1) 1001 | endEffectorPointsXYZ = np.append(endEffectorPointsXYZ, np.expand_dims( 1002 | zEndEffectorPoints[0:index], axis=1), axis=1) 1003 | 1004 | # Save the array to a numpy object in the root directory 1005 | self.directory = '/data' 1006 | self.workspaceFilename = '/EEZYbotARM_workspace/workspace_endEffectorPointsXYZ.npy' 1007 | pathlib.Path(self.directory).mkdir(parents=True, exist_ok=True) 1008 | # .npy extension is added if not given 1009 | np.save(self.directory+self.workspaceFilename, endEffectorPointsXYZ) 1010 | print("Numpy array of workspace points saved to {} as {}".format( 1011 | self.directory, self.workspaceFilename)) 1012 | 1013 | return endEffectorPointsXYZ 1014 | 1015 | def createWorkspaceConvexHull(self): 1016 | 1017 | # Load the numpy array of sampled workspace end effector points 1018 | print("Loading numpy array {}{}".format( 1019 | self.directory, self.workspaceFilename)) 1020 | points = np.load(self.directory+self.workspaceFilename) 1021 | 1022 | print(points.shape) 1023 | 1024 | # Calculate the complex hull! 1025 | hull = ConvexHull(points) 1026 | 1027 | # create the figure 1028 | 1029 | # fig_workspace = plt.figure() 1030 | 1031 | # #add an axis 1032 | # ax = fig_workspace.add_subplot(111, projection='3d') 1033 | rows, cols = hull.simplices.shape 1034 | 1035 | # Plot surface traingulation 1036 | simplical_facet_corners = [] 1037 | 1038 | for counter, simplex in enumerate(hull.simplices): 1039 | simplical_facet_corners.append( 1040 | [points[simplex[0], :], points[simplex[1], :], points[simplex[2], :]]) 1041 | 1042 | # Define directory to save convex hull 1043 | self.directory = '/data' 1044 | self.convexHullFilename = '/EEZYbotARM_workspace/workspace_convexHull.p' 1045 | 1046 | with open(self.directory+self.convexHullFilename, 'wb') as fp: 1047 | pickle.dump(simplical_facet_corners, fp) 1048 | 1049 | # Plotting preperation 1050 | ax = a3.Axes3D(pl.figure()) 1051 | # facetCol = sp.rand(3) #[0.0, 1.0, 0.0] 1052 | facetCol = [0.5, 0.8, 0.4] 1053 | lineCol = [0.8, 0.8, 0.8, 0.5] 1054 | 1055 | # Plot surface traingulation 1056 | for simplex in hull.simplices: 1057 | vtx = [points[simplex[0], :], 1058 | points[simplex[1], :], points[simplex[2], :]] 1059 | tri = a3.art3d.Poly3DCollection([vtx], linewidths=1, alpha=0.4) 1060 | tri.set_color(facetCol) 1061 | tri.set_edgecolor(lineCol) 1062 | ax.add_collection3d(tri) 1063 | 1064 | # Set limits for plot 1065 | ax.set_xlim([-350, 350]) 1066 | ax.set_ylim([-350, 350]) 1067 | ax.set_zlim([-350, 350]) 1068 | 1069 | # plt.axis('off') 1070 | plt.show() 1071 | 1072 | 1073 | # ------------------------------------------# 1074 | # Child class [inherits from EEZYbotARM() class] 1075 | # ------------------------------------------# 1076 | 1077 | class EEZYbotARM_Mk2(EEZYbotARM): 1078 | """ 1079 | --Description-- 1080 | This is a child class for the EEzybot Robotic arm MK2 (inherits from EEZYbotARM() class) 1081 | 1082 | **Please note that you must always instantiate on a child class (either EEZYbotARMMk2() or EEZYbotARMMk1()) 1083 | rather than instansiating the parent 'EEZYbotARM' class** 1084 | 1085 | --Methods-- 1086 | Description of available methods in this class: 1087 | - q3CalcLimits --> Calculate the physical limits for joint 3 (because these depend on the angle of joint 2) 1088 | - map_kinematicsToServoAngles --> Map angles defined in kinematics to physical servo angles on the robot 1089 | """ 1090 | 1091 | # Class attribute 1092 | # DH parameters (Proximal Convention) 1093 | L1 = 92 # mm 1094 | L2 = 135 1095 | L3 = 147 1096 | L4 = 87 1097 | 1098 | # --- Lengths of links which attach to the hoarm 1099 | L2A = 60 1100 | LAB = 140 1101 | LB3 = 60 1102 | 1103 | # Joint limits 1104 | q1_min = -30 # degrees 1105 | q1_max = 30 1106 | q2_min = 39 1107 | q2_max = 120 1108 | # q3_min, q3_max are given by q3CalcLimits() function 1109 | 1110 | def q3CalcLimits(self, **kwargs): 1111 | """ 1112 | Calculate q3 angle limits for the EzzyBot Arm given a value for the angle q2 in degrees 1113 | These limits have been determined experimentally for the EEzybotMk2 1114 | 1115 | If no q2 value is given then the current value of q2 is used 1116 | 1117 | --Optional kwargs Parameters-- 1118 | @q2 -> the value of the angle q2 (in degrees) 1119 | 1120 | --Returns-- 1121 | q3_min, q3_max -> the min and max limits for the angle q3 (in degrees) 1122 | 1123 | """ 1124 | # Use **kwarg if provided, otherwise use current q2 value 1125 | q2 = kwargs.get('q2', self.q2) 1126 | 1127 | # calculate q3 min limits in degrees 1128 | q3_min = (-0.6755 * q2) - 70.768 1129 | q3_max = (-0.7165 * q2) - 13.144 1130 | 1131 | return q3_min, q3_max 1132 | 1133 | def map_kinematicsToServoAngles(self, **kwargs): 1134 | """ 1135 | --Description-- 1136 | Take the three angles for the EzzyBot Arm defined in kinematics. 1137 | Use these to calculate the required physical servo position with respect to the reference position. 1138 | If three angles are not provided as **kwargs then the current values for the arm are used 1139 | 1140 | The reference positions for the three servos are as follows: 1141 | 1142 | EzzyBot base (q1) : 90 degree servo position is facing directly forwards 1143 | Main arm (q2): 90 degree servo position is with main arm perpendicular (at 90 degrees to) base 1144 | Horarm (q3): 90 degree servo poisition is with horarm servo link at 45 degrees to base 1145 | 1146 | The function will be updated to raise an error message when any of the returned angles are outside of the servo limits. 1147 | 1148 | --Optional **kwargs Parameters-- 1149 | @q1 -> the value of the angle q1 (in degrees) as used in the kinematics model 1150 | @q2 -> the value of the angle q2 (in degrees) as used in the kinematics model 1151 | @q3 -> the value of the angle q3 (in degrees) as used in the kinematics model 1152 | 1153 | --Returns-- 1154 | servoAngle_q1, servoAngle_q2, servoAngle_q3 -> values in degrees for output to the physical servos 1155 | 1156 | """ 1157 | 1158 | # Use **kwargs if provided, otherwise use current values 1159 | q1 = kwargs.get('q1', self.q1) 1160 | q2 = kwargs.get('q2', self.q2) 1161 | q3 = kwargs.get('q3', self.q3) 1162 | 1163 | # Check none of the angles are outside of joint limits! So that servos cannot get damaged 1164 | self.checkErrorJointLimits(q1=q1, q2=q2, q3=q3) 1165 | 1166 | # Calculate for q1 1167 | servoAngle_q1 = ((-2.0497)*q1) + 91.726 # from experimentation ! 1168 | servoAngle_q1 = round(servoAngle_q1, 2) 1169 | 1170 | # Calculate for q2 1171 | servoAngle_q2 = 180 - q2 # approximate adjusted q2 value 1172 | servoAngle_q2 = round(servoAngle_q2, 2) 1173 | 1174 | # Calculate for q3 1175 | q3_a = 180 - (- q3) # approximate adjusted q3 value 1176 | servoAngle_q3 = q2 - 45 + q3_a 1177 | servoAngle_q3 = round(servoAngle_q3, 2) 1178 | 1179 | return servoAngle_q1, servoAngle_q2, servoAngle_q3 1180 | 1181 | 1182 | class EEZYbotARM_Mk1(EEZYbotARM): 1183 | """ 1184 | --Description-- 1185 | This is a child class for the EEzybot Robotic arm MK1 (inherits from EEZYbotARM() class) 1186 | 1187 | **Please note that you must always instantiate on a child class (either EEZYbotARMMk2() or EEZYbotARMMk1()) 1188 | rather than instansiating the parent 'EEZYbotARM' class** 1189 | 1190 | --Methods-- 1191 | Description of available methods in this class: 1192 | - q3CalcLimits --> Calculate the physical limits for joint 3 (because these depend on the angle of joint 2) 1193 | - map_kinematicsToServoAngles --> Map angles defined in kinematics to physical servo angles on the robot 1194 | """ 1195 | 1196 | # Class attribute 1197 | # DH parameters (Proximal Convention) 1198 | L1 = 61 # mm 1199 | L2 = 80 1200 | L3 = 80 1201 | L4 = 57 1202 | 1203 | # --- Lengths of links which attach to the hoarm 1204 | L2A = 35 1205 | LAB = 80 1206 | LB3 = 35 1207 | 1208 | # Joint limits 1209 | q1_min = -30 # degrees 1210 | q1_max = 30 1211 | q2_min = 39 1212 | q2_max = 120 1213 | # q3_min, q3_max are given by q3CalcLimits() function 1214 | 1215 | def q3CalcLimits(self, **kwargs): 1216 | """ 1217 | Calculate q3 angle limits for the EzzyBot Arm given a value for the angle q2 in degrees 1218 | These limits have been determined experimentally for the EEzybotMk1 1219 | 1220 | If no q2 value is given then the current value of q2 is used 1221 | 1222 | --Optional kwargs Parameters-- 1223 | @q2 -> the value of the angle q2 (in degrees) 1224 | 1225 | --Returns-- 1226 | q3_min, q3_max -> the min and max limits for the angle q3 (in degrees) 1227 | 1228 | """ 1229 | # Use **kwarg if provided, otherwise use current q2 value 1230 | q2 = kwargs.get('q2', self.q2) 1231 | 1232 | # calculate q3 min limits in degrees 1233 | q3_min = (-0.6755 * q2) - 70.768 1234 | q3_max = (-0.7165 * q2) - 13.144 1235 | 1236 | return q3_min, q3_max 1237 | 1238 | def map_kinematicsToServoAngles(self, **kwargs): 1239 | """ 1240 | --Description-- 1241 | Take the three angles for the EzzyBot Arm defined in kinematics. 1242 | Use these to calculate the required physical servo position with respect to the reference position. 1243 | If three angles are not provided as **kwargs then the current values for the arm are used 1244 | 1245 | The reference positions for the three servos are as follows: 1246 | 1247 | EzzyBot base (q1) : 90 degree servo position is facing directly forwards 1248 | Main arm (q2): 90 degree servo position is with main arm perpendicular (at 90 degrees to) base 1249 | Horarm (q3): 90 degree servo poisition is with horarm servo link at 45 degrees to base 1250 | 1251 | The function will be updated to raise an error message when any of the returned angles are outside of the servo limits. 1252 | 1253 | --Optional **kwargs Parameters-- 1254 | @q1 -> the value of the angle q1 (in degrees) as used in the kinematics model 1255 | @q2 -> the value of the angle q2 (in degrees) as used in the kinematics model 1256 | @q3 -> the value of the angle q3 (in degrees) as used in the kinematics model 1257 | 1258 | --Returns-- 1259 | servoAngle_q1, servoAngle_q2, servoAngle_q3 -> values in degrees for output to the physical servos 1260 | 1261 | """ 1262 | 1263 | # Use **kwargs if provided, otherwise use current values 1264 | q1 = kwargs.get('q1', self.q1) 1265 | q2 = kwargs.get('q2', self.q2) 1266 | q3 = kwargs.get('q3', self.q3) 1267 | 1268 | # Check none of the angles are outside of joint limits! So that servos cannot get damaged 1269 | self.checkErrorJointLimits(q1=q1, q2=q2, q3=q3) 1270 | 1271 | # Calculate for q1 1272 | servoAngle_q1 = ((-2.0497)*q1) + 91.726 # from experimentation ! 1273 | servoAngle_q1 = round(servoAngle_q1, 2) 1274 | 1275 | # Calculate for q2 1276 | servoAngle_q2 = 180 - q2 # approximate adjusted q2 value 1277 | servoAngle_q2 = round(servoAngle_q2, 2) 1278 | 1279 | # Calculate for q3 1280 | q3_a = 180 - (- q3) # approximate adjusted q3 value 1281 | servoAngle_q3 = q2 - 45 + q3_a 1282 | servoAngle_q3 = round(servoAngle_q3, 2) 1283 | 1284 | return servoAngle_q1, servoAngle_q2, servoAngle_q3 1285 | 1286 | 1287 | # # ------------------------------------------# 1288 | # # Function for plotting robot arm 1289 | # # ------------------------------------------# 1290 | 1291 | # class plot_EEZYbotARM(robot_arm): 1292 | -------------------------------------------------------------------------------- /python_packages/easyEEZYbotARM/serial_communication.py: -------------------------------------------------------------------------------- 1 | 2 | #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 3 | # Program Title: serial_communication.py 4 | # Program Purpose: Manages serial communication between a computer (running python) and Arduino for the purpose of controlling the EEXYbotARM 5 | 6 | # **Version control** 7 | # v3.5 -> moving to github 8 | 9 | #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# 10 | # ------------------------------------------# 11 | # Imports # 12 | # ------------------------------------------# 13 | 14 | import serial 15 | import time 16 | 17 | # ------------------------------------------# 18 | # Main class # 19 | # ------------------------------------------# 20 | 21 | 22 | class arduinoController: 23 | """ 24 | --Description-- 25 | This is a class for the EEZYBotARM2Arduino object 26 | 27 | This objects facilitates the creation of a serial connection to the Arduino (or another microcontroller) for the purpose of controling the 28 | three joint angles (and hence the position of the robot arm end effector) 29 | 30 | Credit: the methods in this function are primarily attributed to 'Robin2' in the Arduino forums 31 | 32 | --Methods-- 33 | Description of available methods in this class: 34 | - __init__ --> Initialise an object instance 35 | - connectToSerialPort--> Connects to Arduino serial port using pySerial. 36 | - closeSerialPort--> Close serial port using pySerial. 37 | - sendToArduino --> Sends data to Arduino 38 | - recvFromArduino --> Receive data from Arduino 39 | - waitForArduino --> Function waits until the Arduino sends 'Arduino Ready' - allows time for Arduino reset 40 | - composeMessage --> Function composes a message (instruction) to send to the Arduino, this tells it where to position 41 | the connected servo motors 42 | - runTest --> Function runs a test to check communications with Arduino! 43 | """ 44 | 45 | # Class attributes 46 | startMarker = 60 # unicode character '<' 47 | endMarker = 62 # unicode character '>' 48 | servoTime1 = 1000 # default times of servo movement 49 | servoTime2 = 1000 50 | servoTime3 = 1000 51 | servoTimeEE = 500 52 | msg = "" # default message 53 | 54 | # Initializer / Instance attributes 55 | 56 | def __init__(self, port="COM18"): 57 | self.port = port 58 | 59 | # Include function here connect and pass joint angle 60 | 61 | def __waitForArduino__(self): 62 | """ 63 | --Description-- 64 | Function waits until the Arduino sends 'Arduino Ready' - allows time for Arduino reset. 65 | It also ensures that any bytes left over from a previous message are discarded 66 | 67 | --Parameters-- 68 | None 69 | 70 | --Returns-- 71 | Function doesn't return a value 72 | 73 | """ 74 | 75 | msg = "" 76 | while msg.find("Arduino is ready") == -1: 77 | 78 | # note changed to in_waiting, get the number of bytes in the input buffer 79 | while self.serialPort.inWaiting() == 0: 80 | pass 81 | 82 | msg = self.recvFromArduino() 83 | 84 | print(msg) 85 | print("") 86 | 87 | # Instance methods 88 | def openSerialPort(self, baudRate=9600, port=None): 89 | """ 90 | --Description-- 91 | Connects to a serial port using pySerial. 92 | This function exists to avoid confusion in naming of serial port! 93 | 94 | --Parameters-- 95 | @myString -> the string to be sent 96 | 97 | --Returns-- 98 | Function doesn't return a value 99 | 100 | """ 101 | if port is None: 102 | port = self.port 103 | 104 | self.serialPort = serial.Serial(port=self.port, baudrate=baudRate) 105 | print("Serial port " + port + " opened Baudrate " + str(baudRate)) 106 | 107 | # wait for arduino to be ready 108 | self.__waitForArduino__() 109 | 110 | def closeSerialPort(self, port=None): 111 | """ 112 | --Description-- 113 | Close serial port using pySerial. 114 | This function exists to avoid confusion in naming of serial port! 115 | 116 | --Parameters-- 117 | @myString -> the string to be sent 118 | 119 | --Returns-- 120 | Function doesn't return a value 121 | 122 | """ 123 | if port is None: 124 | port = self.port 125 | 126 | self.serialPort.close 127 | print("Serial port " + port + " closed") 128 | 129 | def sendToArduino(self, myString=None): 130 | """ 131 | --Description-- 132 | Sends a string to the Arduino. String must be in the format "" [HOLD] 133 | 134 | --Parameters-- 135 | @myString -> the string to be sent 136 | 137 | --Returns-- 138 | Function doesn't return a value 139 | 140 | """ 141 | if myString is None: 142 | myString = self.msg 143 | 144 | self.serialPort.write(myString.encode('utf-8')) # encode as unicode 145 | 146 | def recvFromArduino(self): 147 | """ 148 | --Description-- 149 | Recieve a message from the Arduino. The message is interpreted as a string being 150 | between the start character '<' and end character '>' 151 | 152 | --Parameters-- 153 | None 154 | 155 | --Returns-- 156 | msg -> The received message as a string 157 | 158 | """ 159 | msg = "" 160 | x = "z" # any value that is not an end or startMarker 161 | byteCount = -1 # to allow for the fact that the last increment will be one too many 162 | 163 | # wait for the start character 164 | while ord(x) != self.startMarker: # ord returns the unicode number for the character 165 | x = self.serialPort.read() 166 | 167 | # save data until the end marker is found 168 | while ord(x) != self.endMarker: 169 | if ord(x) != self.startMarker: 170 | msg = msg + x.decode("utf-8") # decode from unicode 171 | byteCount += 1 172 | x = self.serialPort.read() 173 | 174 | return(msg) 175 | 176 | def composeMessage(self, servoAngle_q1, servoAngle_q2, servoAngle_q3, servoAngle_EE=90, instruction="BUZZ", **kwargs): 177 | """ 178 | --Description-- 179 | Function composes a message (instruction) to send to the Arduino, this tells it where to position 180 | the connected servo motors. As input we take the servo angles and optionally the movement durations 181 | for each angle 182 | 183 | This function can take direct output (for joint angles q1, q2, q3) from the function 'map_kinematicsToServoAngles()' 184 | 185 | --Parameters-- 186 | @servoAngle_q1 -> the servo angle for servo #1, corresponding to q1 187 | @servoAngle_q2 -> the servo angle for servo #2, corresponding to q2 188 | @servoAngle_q3 -> the servo angle for servo #3, corresponding to q3 189 | @servoAngle_EE -> the servo angle for servo #0, corresponding to the end effector 190 | @instruction -> the text instruction to send to the Arduino (BUZZ-> sounds buzzer), (LED -> flashes LED) 191 | 192 | --Optional **kwargs parameters-- 193 | @servoTime1 -> the time of the servo1 movement (if change in value is input for servo1Angle) 194 | @servoTime2 -> the time of the servo2 movement (if change in value is input for servo2Angle) 195 | @servoTime3 -> the time of the servo3 movement (if change in value is input for servo3Angle) 196 | @servoTimeEE -> the time of the servoEE movement (if change in value is input for servoEEAngle) 197 | 198 | --Returns-- 199 | Function doesn't return a value 200 | 201 | """ 202 | 203 | # Use **kwargs if provided, otherwise use current values 204 | servoTime1 = str(kwargs.get('servoTime1', self.servoTime1)) 205 | servoTime2 = str(kwargs.get('servoTime2', self.servoTime2)) 206 | servoTime3 = str(kwargs.get('servoTime3', self.servoTime3)) 207 | servoTimeEE = str(kwargs.get('servoTimeEE', self.servoTimeEE)) 208 | 209 | # Compose message 210 | message = "<" + ",".join([instruction, str(servoAngle_EE), 211 | str(servoAngle_q1), str(servoAngle_q2), str(servoAngle_q3)]) 212 | message = message + "," + \ 213 | ",".join([servoTimeEE, servoTime1, servoTime2, servoTime3]) + ">" 214 | 215 | self.msg = message 216 | 217 | return message 218 | 219 | def communicate(self, data, delay_between_commands=5): 220 | """ 221 | --Description-- 222 | Function runs a test to check communications with Arduino! 223 | 224 | --Parameters-- 225 | data-> a command string (returned by the composeMessage method) or a list of strings, which will be communicated to the arduino 226 | delay_between_commands -> the time delay applied between a list of commands 227 | 228 | --Returns-- 229 | Function doesn't return a value 230 | 231 | """ 232 | # convert data to list if it isn't already a list 233 | if not isinstance(data, list): 234 | data = [data] 235 | 236 | # Declare variables 237 | numLoops = len(data) 238 | waitingForReply = False 239 | n = 0 240 | 241 | while n < numLoops: 242 | 243 | data_str = data[n] 244 | 245 | if waitingForReply == False: 246 | self.sendToArduino(data_str) 247 | print("Sent from PC -- LOOP NUM " + 248 | str(n) + " TEST STR " + data_str) 249 | waitingForReply = True 250 | 251 | if waitingForReply == True: 252 | 253 | while self.serialPort.inWaiting() == 0: 254 | pass 255 | 256 | dataRecvd = self.recvFromArduino() 257 | print("Reply Received " + dataRecvd) 258 | n += 1 259 | waitingForReply = False 260 | 261 | print("===========") 262 | 263 | time.sleep(delay_between_commands) 264 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/requirements.txt -------------------------------------------------------------------------------- /resources/example_servo_calibration.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meisben/easyEEZYbotARM/00ff522d6bbb2fa6426b9888dfed810f6b0b7bcc/resources/example_servo_calibration.xlsx -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """Setup file for easyEEZYbotARM 3 | """ 4 | 5 | from setuptools import setup 6 | 7 | with open("README.md", "r") as f: 8 | long_description = f.read() 9 | 10 | setup( 11 | name="easyEEZYbotARM", 12 | version="0.0.1", 13 | description="A python controller (3 dimensions inverse and forward kinematics) for the EEZYbotARM (MK1,MK2,MK3) movement", 14 | license="MIT", 15 | long_description=long_description, 16 | author="Ben Money-Coomes", 17 | author_email="ben.money@gmail.com", 18 | url="https://github.com/meisben", 19 | package_dir={'': 'python_packages'}, 20 | packages=["easyEEZYbotARM", ], 21 | install_requires=["scipy", "numpy", "matplotlib", "pySerial"] 22 | ) 23 | --------------------------------------------------------------------------------