diff --git a/Project.code-workspace b/Project.code-workspace new file mode 100644 index 0000000..0107317 --- /dev/null +++ b/Project.code-workspace @@ -0,0 +1,12 @@ +{ + "folders": [ + { + "path": "../../Kinematics/Project" + }, + { + "name": "TestHX711", + "path": "C:/Users/putte/Documents/PlatformIO/Projects/TestHX711" + } + ], + "settings": {} +} \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..3af1d27 --- /dev/null +++ b/README.md @@ -0,0 +1,68 @@ +# Quadcopter Simulation +> This project aims to visualize a Quadcopter's motion (roll, pitch, yaw, and hover) in 3D space by adjusting the speed of four motors using Python. The project consists of three main parts: motor speed adjustment window, dynamic calculation, and 3D visualization. Special thanks to [this MATLAB script](https://youtu.be/4hlQ2pf842U?si=a1AfHnj8r89j6BRX) for finding the Quadcopter's equation of motion. +# Table of Contents +> - [**Installation**](#installation) +> - [**Component**](#component) +> - [**User Guide**](#userguide) +> - [**Demos & Result**](#demosnresult) +> - [**Conclusion**](#conclusion) +> - [**Reference**](#reference) +## Installation + +### Pygame + +> 1. Open Visual Studio Code. +> 2. Go to the terminal or open a new terminal. +> 3. Copy and paste the following command: +> ```bash +> pip install pygame +> ``` +> 4. Press Enter and wait for the download to complete. + +### OpenGL + +> 1. Open Visual Studio Code. +> 2. Go to the terminal or open a new terminal. +> 3. Copy and paste the following command: +> ```bash +> pip install PyOpenGL +> pip install PyOpenGL_accelerate +> ``` +> 4. Press Enter and wait for the download to complete. + +### Numpy + +> 1. Open Visual Studio Code. +> 2. Go to the terminal or open a new terminal. +> 3. Copy and paste the following command: +> ```bash +> pip install numpy +> ``` +> 4. Press Enter and wait for the download to complete. + +# Component +> - **Motor Slider** +> Description +> +> - **Dynamic Calculation** +> > +> - **3D Visualization** +> >The visualization part involves drawing the Quadcopter on the screen and updating its position based on differential values of X, Y, Z, roll, pitch, yaw. +> //Add picture +> > ![a](https://ibb.co/GdqvMGP) +> > +> >The libraries used for this visualization are Pygame for display creation and OpenGL for graphics rendering. +# User Guide +> Description +# Demos & Result +> Description +# Conclusion +> Description +# Reference +> - [1] Lebedev, A. (2013). Design and Implementation of a 6DOF Control System for an Autonomous Quadrocopter (Master's thesis). Julius Maximilian University of Würzburg, Faculty of Mathematics and Computer Science, Aerospace Information Technology, Chair of Computer Science VIII, Prof. Dr. Sergio Montenegro. +> - [2] DRONE OMEGA, 2020, What is a Quadcopter Explained Thoroughly [Online], Available: [droneomega.com](https://droneomega.com/what-is-a-quadcopter/) [02/11/23] +> - [3] Pranav Bhounsule, 2020, Robotics Lec25,26: 3D quadcopter, derivation, simulation, animation (Fall 2020) [Online], Available: [YouTube](https://www.youtube.com/watch?v=4hlq2pf842u) [02/11/23] +> - [4] MATLAB, 2020, Drone Simulation and Control, Part 1: Setting Up the Control Problem [Online], Available: [YouTube](https://www.youtube.com/watch?v=hgcgpuqb67q) [02/11/23] +> - [5] Kanishke Gamagedara (2021). Plotting 3D Objects with Matplotlib. Github. https://github.com/kanishkegb/pyplot-3d +> - [6] P. Wang, Z. Man, Z. Cao, J. Zheng and Y. Zhao, "Dynamics modelling and linear control of quadcopter," 2016 International Conference on Advanced Mechatronic Systems (ICAMechS), Melbourne, VIC, Australia, 2016, pp. 498-503, doi: 10.1109/ICAMechS.2016.7813499. +> - [7] Ahmad, F., Kumar, P., & Patil, P. P. (2018). Modeling and simulation of a quadcopter with altitude and attitude control. Nonlinear Studies, 25(2), 287–299. diff --git a/animation.gif b/animation.gif new file mode 100644 index 0000000..84c706c Binary files /dev/null and b/animation.gif differ diff --git a/pyplot3d/.gitattributes b/pyplot3d/.gitattributes new file mode 100644 index 0000000..ddbe473 --- /dev/null +++ b/pyplot3d/.gitattributes @@ -0,0 +1,148 @@ +# LaTeX +*.tex text eol=lf +*.cls text eol=lf +*.sty text eol=lf +*.bst text eol=lf +*.bib text eol=lf + +#sources +*.c text eol=lf +*.cc text eol=lf +*.cxx text eol=lf +*.cpp text eol=lf +*.c++ text eol=lf +*.hpp text eol=lf +*.h text eol=lf +*.h++ text eol=lf +*.hh text eol=lf + +# Compiled Object files +*.slo binary +*.lo binary +*.o binary +*.obj binary + +# Precompiled Headers +*.gch binary +*.pch binary + +# Compiled Dynamic libraries +*.so binary +*.dylib binary +*.dll binary + +# Compiled Static libraries +*.lai binary +*.la binary +*.a binary +*.lib binary + +# Executables +*.exe binary +*.out binary +*.app binary + + +#common settings that generally should always be used with your language specific settings + +# Auto detect text files and perform LF normalization +# http://davidlaing.com/2012/09/19/customise-your-gitattributes-to-become-a-git-ninja/ +* text=auto + +# +# The above will handle all files NOT found below +# + +# Documents +*.doc diff=astextplain +*.DOC diff=astextplain +*.docx diff=astextplain +*.DOCX diff=astextplain +*.dot diff=astextplain +*.DOT diff=astextplain +*.pdf diff=astextplain +*.PDF diff=astextplain +*.rtf diff=astextplain +*.RTF diff=astextplain +*.md text eol=lf +*.adoc text +*.textile text +*.mustache text +*.csv text eol=lf +*.tab text eol=lf +*.tsv text eol=lf +*.sql text eol=lf + +# Graphics +*.png binary +*.jpg binary +*.jpeg binary +*.gif binary +*.tif binary +*.tiff binary +*.ico binary +*.svg binary +*.eps binary +# Basic .gitattributes for a MATLAB repo. +# This template includes Simulink and MuPAD extensions, in addition +# to the MATLAB extensions. + +# Source files +# ============ +*.m text eol=lf +*.mu text eol=lf + +# Caution: *.m also matches Mathematica packages. + +# Binary files +# ============ +*.p binary +*.mex* binary +*.fig binary +*.mat binary +*.mdl binary +*.slx binary +*.mdlp binary +*.slxp binary +*.sldd binary +*.mltbx binary +*.mlappinstall binary +*.mlpkginstall binary +*.mn binary +# Basic .gitattributes for a python repo. + +# Source files +# ============ +*.pxd text eol=lf +*.py text eol=lf +*.py3 text eol=lf +*.pyw text eol=lf +*.pyx text eol=lf + +# Binary files +# ============ +*.db binary +*.p binary +*.pkl binary +*.pyc binary +*.pyd binary +*.pyo binary + +# Note: .db, .p, and .pkl files are associated +# with the python modules ``pickle``, ``dbm.*``, +# ``shelve``, ``marshal``, ``anydbm``, & ``bsddb`` +# (among others).# Auto detect text files and perform LF normalization +# http://davidlaing.com/2012/09/19/customise-your-gitattributes-to-become-a-git-ninja/ +* text=auto + +# Custom for Visual Studio +*.sln text eol=crlf +*.csproj text eol=crlf +*.vbproj text eol=crlf +*.fsproj text eol=crlf +*.dbproj text eol=crlf + +*.vcxproj text eol=crlf +*.vcxitems text eol=crlf +*.props text eol=crlf +*.filters text eol=crlf \ No newline at end of file diff --git a/pyplot3d/.gitignore b/pyplot3d/.gitignore new file mode 100644 index 0000000..5600b84 --- /dev/null +++ b/pyplot3d/.gitignore @@ -0,0 +1,267 @@ + +# Created by https://www.toptal.com/developers/gitignore/api/python,jupyternotebooks,visualstudiocode,vim,windows,macos,linux +# Edit at https://www.toptal.com/developers/gitignore?templates=python,jupyternotebooks,visualstudiocode,vim,windows,macos,linux + +### JupyterNotebooks ### +# gitignore template for Jupyter Notebooks +# website: http://jupyter.org/ + +.ipynb_checkpoints +*/.ipynb_checkpoints/* + +# IPython +profile_default/ +ipython_config.py + +# Remove previous ipynb_checkpoints +# git rm -r .ipynb_checkpoints/ + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### Python ### +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +parts/ +sdist/ +var/ +wheels/ +pip-wheel-metadata/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +pytestdebug.log + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ +doc/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook + +# IPython + +# pyenv +.python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +#poetry.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +# .env +.env/ +.venv/ +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ +pythonenv* + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# operating system-related files +# file properties cache/storage on macOS +*.DS_Store +# thumbnail cache on Windows +Thumbs.db + +# profiling data +.prof + + +### Vim ### +# Swap +[._]*.s[a-v][a-z] +!*.svg # comment out if you don't need vector files +[._]*.sw[a-p] +[._]s[a-rt-v][a-z] +[._]ss[a-gi-z] +[._]sw[a-p] + +# Session +Session.vim +Sessionx.vim + +# Temporary +.netrwhist +# Auto-generated tag files +tags +# Persistent undo +[._]*.un~ + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +*.code-workspace + +### VisualStudioCode Patch ### +# Ignore all local history of files +.history +.ionide + +### Windows ### +# Windows thumbnail cache files +Thumbs.db:encryptable +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +# End of https://www.toptal.com/developers/gitignore/api/python,jupyternotebooks,visualstudiocode,vim,windows,macos,linux diff --git a/pyplot3d/README.md b/pyplot3d/README.md new file mode 100644 index 0000000..0a53d10 --- /dev/null +++ b/pyplot3d/README.md @@ -0,0 +1,107 @@ +# Plotting 3D Objects with Matplotlib + +A Python class for drawing a 3D objects using Python Matplotlib library. + +![UAV](media/video.gif) + + +The sole purpose of this library is to provide a few customizable 3D shapes so that you can conveneintly use them for generating 3D plots. + +## How to Use + +1. Add this as a submodule. + ```sh + cd /directory/where/you/have/your/python/files + git submodule add https://github.com/kanishkegb/pyplot-3d.git ./pyplot3d + ``` + Alternatively, you can download the repo as a zip file, extract it, rename it to `pyplot3d`, and move it your directory with python codes. + +1. Use the library in your code. + ```python + from pyplot3d.uav import Uav + from pyplot3d.utils import ypr_to_R + + import numpy as np + import matplotlib.pyplot as plt + + plt.style.use('seaborn') + + # initialize plot + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + + arm_length = 0.24 # in meters + uav = Uav(ax, arm_length) + + uav.draw_at([1, 0, 0], ypr_to_R([np.pi/2.0, 0, 0])) + + plt.show() + ``` + +1. Run the code. + ```sh + python3 /name/of/your/file + ``` + +You can combine multiple objects. +For example, the UAV plot is simply a combination of a few other basic shapes defined in `basic.py`. +Just make sure to call all the objects before calling `plt.show()`. + + +Also, you can use it with combination of animations package. +The below code can be used to generate the GIF shown above. + +```python +import numpy as np +from matplotlib import pyplot as plt +# %matplotlib inline + +from matplotlib import animation + +from uav import pyplot3d.Uav +from utils import pyplot3d.ypr_to_R + + +def update_plot(i, x, R): + uav_plot.update_plot(x[:, i], R[:, :, i]) + + # These limits must be set manually since we use + # a different axis frame configuration than the + # one matplotlib uses. + xmin, xmax = -2, 2 + ymin, ymax = -2, 2 + zmin, zmax = -2, 2 + + ax.set_xlim([xmin, xmax]) + ax.set_ylim([ymax, ymin]) + ax.set_zlim([zmax, zmin]) + + +plt.style.use('seaborn') + +fig = plt.figure() +ax = fig.gca(projection='3d') + +arm_length = 0.24 # in meters +uav_plot = Uav(ax, arm_length) + + +# Create some fake simulation data +steps = 60 +t_end = 1 + +x = np.zeros((3, steps)) +x[0, :] = np.arange(0, t_end, t_end / steps) +x[1, :] = np.arange(0, t_end, t_end / steps) * 2 + +R = np.zeros((3, 3, steps)) +for i in range(steps): + ypr = np.array([i, 0.1 * i, 0.0]) + R[:, :, i] = ypr_to_R(ypr, degrees=True) + +ani = animation.FuncAnimation(fig, update_plot, frames=20, fargs=(x, R,)); + +# If using Jupyter Notebooks +# from IPython.display import HTML +# HTML(ani.to_jshtml()) + diff --git a/pyplot3d/__init__.py b/pyplot3d/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyplot3d/basic.py b/pyplot3d/basic.py new file mode 100644 index 0000000..afc2509 --- /dev/null +++ b/pyplot3d/basic.py @@ -0,0 +1,470 @@ +import matplotlib.pyplot as plt +import numpy as np + +from mpl_toolkits.mplot3d import Axes3D + +from .utils import ypr_to_R + + + +class Sphere: + ''' + Draws a sphere at a given position. + ''' + + def __init__(self, ax, r, c='b', x0=np.array([0, 0, 0]).T, resolution=20): + ''' + Initialize the sphere. + + Params: + ax: (matplotlib axis) the axis where the sphere should be drawn + r: (float) radius of the sphere + c: (string) color of the sphere, default 'b' + x0: (3x1 numpy.ndarray) initial position of the sphere, default + is [0, 0, 0] + resolution: (int) resolution of the plot, default 20 + + Returns: + None + ''' + + self.ax = ax + self.r = r + self.color = c + self.x0 = x0 + self.reso = resolution + + + def draw(self): + ''' + Draw the sphere with the initially defined position when the class was + instantiated. + + Args: + None + + Returns: + None + ''' + + vertices = np.linspace(0, 2*np.pi, self.reso+1) + u, v = np.meshgrid(vertices, vertices) + + x = self.r * np.cos(u) * np.sin(v) + self.x0[0] + y = self.r * np.sin(u) * np.sin(v) + self.x0[1] + z = self.r * np.cos(v) + self.x0[2] + + self.ax.plot_surface(x, y, z, color=self.color) + + + def draw_at(self, position=np.array([0.0, 0.0, 0.0]).T): + ''' + Draw the sphere at a given position. + + Args: + position: (3x1 numpy.ndarray) position of the sphere, + default = [0.0, 0.0, 0.0] + + Returns: + None + ''' + + vertices = np.linspace(0, 2*np.pi, self.reso+1) + u, v = np.meshgrid(vertices, vertices) + + x = self.r * np.cos(u) * np.sin(v) + position[0] + y = self.r * np.sin(u) * np.sin(v) + position[1] + z = self.r * np.cos(v) + position[2] + + self.ax.plot_surface(x, y, z, color=self.color) + + + +class Arrow: + ''' + Draws an arrow at a given position, with a given attitude. + ''' + + def __init__(self, ax, direction, c='b', x0=np.array([0.0, 0.0, 0.0]).T, \ + length=1.0): + ''' + Initialize the arrow. + + Params: + ax: (matplotlib axis) the axis where the arrow should be drawn + direction: (3x1 numpy.ndarray) direction of the arrow + c: (string) color of the arrow, default = 'b' + x0: (3x1 numpy.ndarray) origin of the arrow, + default = [0.0, 0.0, 0.0] + length: (float) length of the arrow, default = 1.0 + + Returns: + None + ''' + + self.ax = ax + self.u0 = direction + self.color = c + self.x0 = x0 + self.arrow_length = length + + + def draw(self): + ''' + Draw the arrow with the initially defined parameter when the class was + instantiated. + + Args: + None + + Returns: + None + ''' + + x = self.x0 + u = self.u0 + + self.ax.quiver(x[0], x[1], x[1], \ + u[0], u[1], u[2], \ + color=self.color, + length=self.arrow_length, \ + normalize=False) + + + def draw_from_to(self, x=np.array([0.0, 0.0, 0.0]).T, \ + u=np.array([1.0, 0.0, 0.0]).T): + ''' + Draw the arrow at a given position, with a given direction + + Args: + x: (3x1 numpy.ndarray) origin of the arrow, + default = [0.0, 0.0, 0.0] + u: (3x1 numpy.ndarray) direction of the arrow, + default = [1.0, 0.0, 0.0] + + Returns: + None + ''' + + self.ax.quiver(x[0], x[1], x[2], \ + u[0], u[1], u[2], \ + color=self.color, + length=self.arrow_length, \ + normalize=False) + + + +class Line: + ''' + Draws a line at a given position, with a given attitude. + ''' + + def __init__(self, ax, c='b', x0=np.array([0.0, 0.0, 0.0]).T, \ + x1=np.array([1.0, 0.0, 0.0]).T): + ''' + Initialize the line. + Params: + ax: (matplotlib axis) the axis where the line should be drawn + c: (string) color of the arrow, default = 'b' + x0: (3x1 numpy.ndarray) start of the line, + default = [0.0, 0.0, 0.0] + x1: (3x1 numpy.ndarray) end of the line, + default = [1.0, 0.0, 0.0] + + Returns: + None + ''' + + self.ax = ax + self.color = c + self.x0 = x0 + self.x1 = x1 + + + def draw(self): + ''' + Draw the line with the initially defined parameter when the class was + instantiated. + Args: + None + + Returns: + None + ''' + + self.ax.plot([self.x0[0], self.x1[0]], \ + [self.x0[1], self.x1[1]], \ + [self.x0[2], self.x1[2]], \ + color=self.color) + + + def draw_from_to(self, x0=np.array([0.0, 0.0, 0.0]).T, \ + x1=np.array([1.0, 0.0, 0.0]).T): + ''' + Draw the line between two points. + Args: + x0: (3x1 numpy.ndarray) start of the line, + default = [0.0, 0.0, 0.0] + x1: (3x1 numpy.ndarray) end of the line, + default = [1.0, 0.0, 0.0] + + Returns: + None + ''' + + self.ax.plot([x0[0], x1[0]], \ + [x0[1], x1[1]], \ + [x0[2], x1[2]], \ + color=self.color) + + +class Plane: + ''' + Draws a plane at a given position. + ''' + + def __init__(self, ax, h, w, c='b', x=np.array([0, 0, 0]).T, \ + R=np.eye(3), resolution=1): + ''' + Initialize the sphere. + Params: + ax: (matplotlib axis) the axis where the plane should be drawn + h = (float): height of the plane (x axis) + w = (float): width of the plane (y axis) + c: (string) color of the plane, default 'b' + x: (3x1 numpy.ndarray) initial position of the plane, default + is [0, 0, 0] + R: (3x1 numpy.ndarray) attitude of the plane, + default = eye(3) + resolution: (int) resolution of the plot, default 1 + + Returns: + None + ''' + + self.ax = ax + self.h = h + self.w = w + self.color = c + self.x = x + self.R = R + self.reso = resolution + + self.uvw = np.array([]) + self.mesh_shape = (1, 1) + + + def draw(self): + ''' + Draw the plane with the initially defined position when the class was + instantiated. + Args: + None + + Returns: + None + ''' + + if self.uvw.size == 0: + reso = self.reso + h = self.h/2.0 + w = self.w/2.0 + + vertices_h = np.linspace(-h, h, reso+1) + vertices_w = np.linspace(-w, w, reso+1) + + u, v = np.meshgrid(vertices_h, vertices_w) + w = u*0.0 + + self.mesh_shape = np.shape(u) + self.uvw = np.array([u.ravel(), v.ravel(), w.ravel()]) + + # NOTE: for higher resolutions, raveling and reshpaing can be + # expensive. Replace this with np.einsum. + uvw = self.R @ self.uvw + + self.ax.plot_surface( + uvw[0, :].reshape(self.mesh_shape) + float(self.x[0]), + uvw[1, :].reshape(self.mesh_shape) + float(self.x[1]), + uvw[2, :].reshape(self.mesh_shape) + float(self.x[2]), + color=self.color) + + + def draw_at(self, x=np.array([0.0, 0.0, 0.0]).T, R=np.eye(3)): + ''' + Draw the plane at a given position and attitude. + + Args: + x: (3x1 numpy.ndarray) position of plane, + default = [0.0, 0.0, 0.0] + R: (3x1 numpy.ndarray) attitude of the plane, + default = eye(3) + + Returns: + None + ''' + + if self.uvw.size == 0: + reso = self.reso + h = self.h/2.0 + w = self.w/2.0 + + vertices_h = np.linspace(-h, h, reso+1) + vertices_w = np.linspace(-w, w, reso+1) + + u, v = np.meshgrid(vertices_h, vertices_w) + w = u*0.0 + + self.mesh_shape = np.shape(u) + self.uvw = np.array([u.ravel(), v.ravel(), w.ravel()]) + + # NOTE: for higher resolutions, raveling and reshpaing can be + # expensive. Replace this with np.einsum. + uvw = R @ self.uvw + + self.ax.plot_surface( + uvw[0, :].reshape(self.mesh_shape) + float(x[0]), + uvw[1, :].reshape(self.mesh_shape) + float(x[1]), + uvw[2, :].reshape(self.mesh_shape) + float(x[2]), + color=self.color) + + + +class Cube: + ''' + Draws a cube at a given position. + ''' + + def __init__(self, ax, dimensions, c='b', x=np.array([0, 0, 0]).T, \ + R=np.eye(3), resolution=10): + ''' + Initialize the cube. + Params: + ax: (matplotlib axis) the axis where the cube should be drawn + dimensions = (3x1 numpy.ndarray): dimensions along each axis + c: (string) color of the cube, default 'b' + x: (3x1 numpy.ndarray) initial position of the cube, default + is [0, 0, 0] + R: (3x1 numpy.ndarray) attitude of the cube, + default = eye(3) + resolution: (int) resolution of the plot, default 10 + + Returns: + None + ''' + + self.ax = ax + self.d1 = dimensions[0] + self.d2 = dimensions[1] + self.d3 = dimensions[2] + self.color = c + self.x = x + self.R = R + self.reso = resolution + + theta = np.pi / 2.0 + + self.pt1 = np.array([self.d1/2.0, 0.0, 0.0]) + self.R1 = ypr_to_R([0.0, theta, 0.0]) + self.p1 = Plane(self.ax, self.d3, self.d2, 'r', \ + self.pt1, self.R1) + + self.pt2 = np.array([-self.d1/2.0, 0.0, 0.0]) + self.R2 = ypr_to_R([0.0, -theta, 0.0]) + self.p2 = Plane(self.ax, self.d3, self.d2, 'r', \ + self.pt2, self.R2) + + self.pt3 = np.array([0.0, self.d2/2.0, 0.0]) + self.R3 = ypr_to_R([0.0, 0.0, -theta]) + self.p3 = Plane(self.ax, self.d1, self.d3, 'r', \ + self.pt3, self.R3) + + self.pt4 = np.array([0.0, -self.d2/2.0, 0.0]) + self.R4 = ypr_to_R([0.0, 0.0, theta]) + self.p4 = Plane(self.ax, self.d1, self.d3, 'r', \ + self.pt4, self.R4) + + self.pt5 = np.array([0.0, 0.0, self.d3/2.0]) + self.R5 = np.eye(3) + self.p5 = Plane(self.ax, self.d1, self.d2, 'r', \ + self.pt5, self.R5) + + self.pt6 = np.array([0.0, 0.0, -self.d3/2.0]) + self.R6 = np.eye(3) + self.p6 = Plane(self.ax, self.d1, self.d2, 'r', \ + self.pt6, self.R6) + + + def draw(self): + ''' + Draw the cube with the initially defined position when the class was + instantiated. + Args: + None + + Returns: + None + ''' + + self.p1.draw() + self.p2.draw() + self.p3.draw() + self.p4.draw() + self.p5.draw() + self.p6.draw() + + + def draw_at(self, x=np.array([0.0, 0.0, 0.0]).T, R=np.eye(3)): + ''' + Draw the camera at a given point and attitude. + Args: + x: (3x1 numpy.ndarray) position of camera, + default = [0.0, 0.0, 0.0] + R: (3x1 numpy.ndarray) attitude of the camera, + default = eye(3) + + Returns: + None + ''' + + raise NotImplementedError('This has not been implemented correctly') + # self.p1.draw_at(x + R@self.R1@self.pt1, R@self.R1) + # self.p2.draw_at(x + R@self.R2@self.pt2, R@self.R2) + # self.p3.draw_at(x + R@self.R3@self.pt3, R@self.R3) + # self.p4.draw_at(x + R@self.R4@self.pt4, R@self.R4) + # self.p5.draw_at(x + R@self.R5@self.pt5, R@self.R5) + # self.p6.draw_at(x + R@self.R6@self.pt6, R@self.R6) + + self.p1.draw_at(x + self.pt1, R@self.R1) + self.p2.draw_at(x + self.pt2, R@self.R2) + self.p3.draw_at(x + self.pt3, R@self.R3) + self.p4.draw_at(x + self.pt4, R@self.R4) + self.p5.draw_at(x + self.pt5, R@self.R5) + self.p6.draw_at(x + self.pt6, R@self.R6) + + + +if __name__ == '__main__': + + # Initiate the plot + plt.style.use('seaborn') + + fig = plt.figure() + ax = fig.gca(projection='3d') + + # s1 = Sphere(ax, 1) + # s1.draw() + + # R = ypr_to_R([0, 0, np.pi/2.0]) + # p1 = Plane(ax, 3, 2, 'r', [0, 0, 1], R) + # p1.draw() + + c1 = Cube(ax, [3, 4, 5]) + # c1.draw_at([1,0,0], ypr_to_R([0,0,0])) + c1.draw_at([0,0,0], ypr_to_R([np.pi/4,0,0])) + + ax.set_xlim([-5, 5]) + ax.set_ylim([-5, 5]) + ax.set_zlim([-5, 5]) + + plt.show() \ No newline at end of file diff --git a/pyplot3d/camera.py b/pyplot3d/camera.py new file mode 100644 index 0000000..2e89324 --- /dev/null +++ b/pyplot3d/camera.py @@ -0,0 +1,120 @@ +from .basic import Line, Sphere + +import numpy as np + + +class Camera: + ''' + Draws a line at a given position, with a given attitude. + ''' + + def __init__(self, ax, c='b', x=np.array([0.0, 0.0, 0.0]).T, R=np.eye(3)): + ''' + Initialize the camera. + Params: + ax: (matplotlib axis) the axis where the line should be drawn + direction: (3x1 numpy.ndarray) direction of the arrow + c: (string) color of the arrow, default = 'b' + x: (3x1 numpy.ndarray) origin of the camera, + default = [0.0, 0.0, 0.0] + R: (3x1 numpy.ndarray) attitude of the camera, + default = eye(3) + + Returns: + None + ''' + + self.ax = ax + self.color = c + self.x = x + self.R = R + + d = 0.3 + w = 0.2 + h = 0.1 + p1 = np.array([d, w, h]) + p2 = np.array([d, -w, h]) + p3 = np.array([d, -w, -h]) + p4 = np.array([d, w, -h]) + self.l1 = Line(self.ax, 'b', x, p1) + self.l2 = Line(self.ax, 'b', x, p2) + self.l3 = Line(self.ax, 'b', x, p3) + self.l4 = Line(self.ax, 'b', x, p4) + self.l5 = Line(self.ax, 'r', p1, p2) + self.l6 = Line(self.ax, 'r', p2, p3) + self.l7 = Line(self.ax, 'r', p3, p4) + self.l8 = Line(self.ax, 'r', p4, p1) + + self.origin = Sphere(self.ax, 0.02, 'y') + + + def draw(self): + ''' + Draw a camera with the initially defined parameter when the class was + instantiated. + Args: + None + + Returns: + None + ''' + + self.l1.draw() + self.l2.draw() + self.l3.draw() + self.l4.draw() + self.l5.draw() + self.l6.draw() + self.l7.draw() + self.l8.draw() + self.origin.draw() + + + def draw_at(self, x=np.array([0.0, 0.0, 0.0]).T, R=np.eye(3)): + ''' + Draw the camera at a given point and attitude. + Args: + x: (3x1 numpy.ndarray) position of camera, + default = [0.0, 0.0, 0.0] + R: (3x1 numpy.ndarray) attitude of the camera, + default = eye(3) + + Returns: + None + ''' + + d = 0.5 + w = 0.4 + h = 0.3 + p1 = x + R@np.array([d, w, h]) + p2 = x + R@np.array([d, -w, h]) + p3 = x + R@np.array([d, -w, -h]) + p4 = x + R@np.array([d, w, -h]) + + self.l1.draw_from_to(x, p1) + self.l2.draw_from_to(x, p2) + self.l3.draw_from_to(x, p3) + self.l4.draw_from_to(x, p4) + self.l5.draw_from_to(p1, p2) + self.l6.draw_from_to(p2, p3) + self.l7.draw_from_to(p3, p4) + self.l8.draw_from_to(p4, p1) + self.origin.draw_at(x) + + + +if __name__ == '__main__': + from utils import ypr_to_R + from mpl_toolkits.mplot3d import Axes3D + + import numpy as np + import matplotlib.pyplot as plt + + plt.style.use('seaborn') + fig = plt.figure() + ax = fig.gca(projection='3d') + + camera = Camera(ax) + camera.draw_at([1, 1, 3], ypr_to_R([0, np.pi/4, 0])) + + plt.show() \ No newline at end of file diff --git a/pyplot3d/media/video.gif b/pyplot3d/media/video.gif new file mode 100644 index 0000000..1c3c943 Binary files /dev/null and b/pyplot3d/media/video.gif differ diff --git a/pyplot3d/media/video.mov b/pyplot3d/media/video.mov new file mode 100644 index 0000000..d8c9713 Binary files /dev/null and b/pyplot3d/media/video.mov differ diff --git a/pyplot3d/uav.py b/pyplot3d/uav.py new file mode 100644 index 0000000..d8589f2 --- /dev/null +++ b/pyplot3d/uav.py @@ -0,0 +1,137 @@ +from .basic import Sphere, Line, Arrow + +import numpy as np + + +class Uav: + ''' + Draws a quadrotor at a given position, with a given attitude. + ''' + + def __init__(self, ax, arm_length): + ''' + Initialize the quadrotr plotting parameters. + + Params: + ax: (matplotlib axis) the axis where the sphere should be drawn + arm_length: (float) length of the quadrotor arm + + Returns: + None + ''' + + self.ax = ax + self.arm_length = arm_length + + self.b1 = np.array([1.0, 0.0, 0.0]).T + self.b2 = np.array([0.0, 1.0, 0.0]).T + self.b3 = np.array([0.0, 0.0, 1.0]).T + + # Center of the quadrotor + self.body = Sphere(self.ax, 0.08, 'y') + + # Each motor + self.motor1 = Sphere(self.ax, 0.05, 'r') + self.motor2 = Sphere(self.ax, 0.05, 'g') + self.motor3 = Sphere(self.ax, 0.05, 'b') + self.motor4 = Sphere(self.ax, 0.05, 'b') + + # Arrows for the each body axis + self.arrow_b1 = Arrow(ax, self.b1, 'r') + self.arrow_b2 = Arrow(ax, self.b2, 'g') + self.arrow_b3 = Arrow(ax, self.b3, 'b') + + # Quadrotor arms + self.arm_b1 = Line(ax) + self.arm_b2 = Line(ax) + + + def draw_at(self, x=np.array([0.0, 0.0, 0.0]).T, R=np.eye(3)): + ''' + Draw the quadrotor at a given position, with a given direction + + Args: + x: (3x1 numpy.ndarray) position of the center of the quadrotor, + default = [0.0, 0.0, 0.0] + R: (3x3 numpy.ndarray) attitude of the quadrotor in SO(3) + default = eye(3) + + Returns: + None + ''' + + # First, clear the axis of all the previous plots + self.ax.clear() + + # Center of the quadrotor + self.body.draw_at(x) + + # Each motor + self.motor1.draw_at(x + R.dot(self.b1) * self.arm_length) + self.motor2.draw_at(x + R.dot(self.b2) * self.arm_length) + self.motor3.draw_at(x + R.dot(-self.b1) * self.arm_length) + self.motor4.draw_at(x + R.dot(-self.b2) * self.arm_length) + + # Arrows for the each body axis + self.arrow_b1.draw_from_to(x, R.dot(self.b1) * self.arm_length * 1.8) + self.arrow_b2.draw_from_to(x, R.dot(self.b2) * self.arm_length * 1.8) + self.arrow_b3.draw_from_to(x, R.dot(self.b3) * self.arm_length * 1.8) + + # Quadrotor arms + self.arm_b1.draw_from_to(x, x + R.dot(-self.b1) * self.arm_length) + self.arm_b2.draw_from_to(x, x + R.dot(-self.b2) * self.arm_length) + + + +if __name__ == '__main__': + from utils import ypr_to_R + + from matplotlib import animation + from mpl_toolkits.mplot3d import Axes3D + + import matplotlib.pyplot as plt + + + def update_plot(i, x, R): + uav_plot.draw_at(x[:, i], R[:, :, i]) + + # These limits must be set manually since we use + # a different axis frame configuration than the + # one matplotlib uses. + xmin, xmax = -2, 2 + ymin, ymax = -2, 2 + zmin, zmax = -2, 2 + + ax.set_xlim([xmin, xmax]) + ax.set_ylim([ymax, ymin]) + ax.set_zlim([zmax, zmin]) + + # Initiate the plot + plt.style.use('seaborn') + + fig = plt.figure() + ax = fig.gca(projection='3d') + + arm_length = 0.24 # in meters + uav_plot = Uav(ax, arm_length) + + + # Create some fake simulation data + steps = 60 + t_end = 1 + + x = np.zeros((3, steps)) + x[0, :] = np.arange(0, t_end, t_end / steps) + x[1, :] = np.arange(0, t_end, t_end / steps) * 2 + + R = np.zeros((3, 3, steps)) + for i in range(steps): + ypr = np.array([i, 0.1 * i, 0.0]) + R[:, :, i] = ypr_to_R(ypr, degrees=True) + + + # Run the simulation + ani = animation.FuncAnimation(fig, update_plot, frames=steps, \ + fargs=(x, R,)) + + plt.show() \ No newline at end of file diff --git a/pyplot3d/utils.py b/pyplot3d/utils.py new file mode 100644 index 0000000..9baecd4 --- /dev/null +++ b/pyplot3d/utils.py @@ -0,0 +1,104 @@ +import numpy as np + + +def rot1(angle, degrees=False): + ''' + Converts pitch angle (a rotation around the 1st body axis) to a rotation + matrix in SO(3). + + Args: + angle: (numpy.ndarray) pitch angle + degrees: (bool) flag to use if the angles are in degrees, + default = False + Returns: + R: (numpy.ndarray) 3x3 rotation matrix in SO(3) + ''' + + if degrees: + angle = np.deg2rad(angle) + + cos_a = np.cos(angle) + sin_a = np.sin(angle) + rot_mat = np.identity(3) + + rot_mat[1, 1] = cos_a + rot_mat[1, 2] = -sin_a + rot_mat[2, 1] = sin_a + rot_mat[2, 2] = cos_a + + return rot_mat + + +def rot2(angle, degrees=False): + ''' + Converts roll angle (a rotation around the 2nd body axis) to a rotation + matrix in SO(3). + + Args: + angle: (numpy.ndarray) roll angle + degrees: (bool) flag to use if the angles are in degrees, + default = False + Returns: + R: (numpy.ndarray) 3x3 rotation matrix in SO(3) + ''' + + if degrees: + angle = np.deg2rad(angle) + + cos_a = np.cos(angle) + sin_a = np.sin(angle) + rot_mat = np.identity(3) + + rot_mat[0, 0] = cos_a + rot_mat[0, 2] = sin_a + rot_mat[2, 0] = -sin_a + rot_mat[2, 2] = cos_a + + return rot_mat + + +def rot3(angle, degrees=False): + ''' + Converts yaw angle (a rotation around the 3rd body axis) to a rotation + matrix in SO(3). + + Args: + angle: (numpy.ndarray) yaw angle + degrees: (bool) flag to use if the angles are in degrees, + default = False + Returns: + R: (numpy.ndarray) 3x3 rotation matrix in SO(3) + ''' + + if degrees: + angle = np.deg2rad(angle) + + cos_a = np.cos(angle) + sin_a = np.sin(angle) + rot_mat = np.identity(3) + + rot_mat[0, 0] = cos_a + rot_mat[0, 1] = -sin_a + rot_mat[1, 0] = sin_a + rot_mat[1, 1] = cos_a + + return rot_mat + + +def ypr_to_R(ypr, degrees=False): + ''' + Converts yaw, pitch, roll angles to a rotation matrix in SO(3). + + Args: + ypr: (numpy.ndarray) 3x1 array with yaw, pitch, roll + degrees: (bool) flag to use if the angles are in degrees, + default = False + Returns: + R: (numpy.ndarray) 3x3 rotation matrix in SO(3) + ''' + + R3 = rot3(ypr[0], degrees) + R2 = rot2(ypr[1], degrees) + R1 = rot1(ypr[2], degrees) + + return R3.dot(R2).dot(R1) \ No newline at end of file diff --git a/sim12.py b/sim12.py new file mode 100644 index 0000000..8e22579 --- /dev/null +++ b/sim12.py @@ -0,0 +1,52 @@ +import numpy as np +from matplotlib import pyplot as plt +from matplotlib import animation +from pyplot3d.uav import Uav +from pyplot3d.utils import ypr_to_R + +def update_plot(i, x, R): + uav_plot.draw_at(x[:, i], R[:, :, i]) #draw on matplotlib + + lim = 2 + + xmin, xmax = -lim, lim + ymin, ymax = -lim, lim + zmin, zmax = -lim, lim + + ax.set_xlim([xmin, xmax]) #set matplotlib window + ax.set_ylim([ymax, ymin]) + ax.set_zlim([zmax, zmin]) + +fig = plt.figure() #init plot +ax = fig.add_subplot(111, projection='3d') + +arm_length = 0.24 # in meters +uav_plot = Uav(ax, arm_length) #init uav + +steps = 1000 +t_end = 300 + +#input xyz +x = np.zeros((3, steps)) + +print(type(x)) +print(x.shape) + +x[0, :] = np.arange(0, t_end, t_end / steps) # x +x[1, :] = np.arange(0, t_end, t_end / steps) * 2 # y +x[2, :] = np.arange(0, t_end, t_end / steps) * -2 # z + +#input ypr +R = np.zeros((3, 3, steps)) + +for i in range(steps): + ypr = np.array([i*5, 0, 0]) #yaw pitch roll + R[:, :, i] = ypr_to_R(ypr, degrees=True) #convert ypr to Rotation matrix + +print(type(ypr)) +print(ypr.shape) +print(type(R)) +print(R.shape) + +ani = animation.FuncAnimation(fig, update_plot, frames=30, fargs=(x, R,)) #make animation +ani.save('animation.gif', writer='pillow', fps=30) #save animation \ No newline at end of file