Created
September 28, 2019 11:20
-
-
Save MikuroXina/a982d91e6fd1ed2c79b2e875747d9885 to your computer and use it in GitHub Desktop.
Simulation of 2 link robot arm
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| using Pkg; | |
| Pkg.add("Plots"); | |
| Pkg.add("GR"); | |
| using Plots; | |
| L = [0.3, 0.3]; | |
| Lg = [0.15, 0.15]; | |
| m = [0.5, 0.5]; | |
| I = [5.4e-3, 5.4e-3]; | |
| mL = 5.0; | |
| g = 9.806199; | |
| dt = 0.001; | |
| A1 = m[1] * Lg[1] ^ 2 + I[1] + (m[2] + mL) * L[1] ^ 2; | |
| A2 = I[2] + m[2] * Lg[2] ^ 2 + mL * L[2] ^ 2; | |
| A3 = (m[2] * Lg[2] + mL * L[2]) * L[1]; | |
| B1 = (m[1] * Lg[1] + (m[2] + mL) * L[1]) * g; | |
| B2 = (m[2] * Lg[2] + mL * L[2]) * g; | |
| let | |
| t = 0; | |
| z = [90π / 180.0, -90π / 180.0]; | |
| zd = z; | |
| w = [0, 0]; | |
| P = [L[2], L[1]]; | |
| V = [0, 0]; | |
| th = zeros(801, 2); | |
| thv = zeros(801, 2); | |
| pos = zeros(801, 4); | |
| for i in 0:800 | |
| t += dt; | |
| P = [ | |
| cos(z[1]) cos(z[1] + z[2]) | |
| sin(z[1]) sin(z[1] + z[2]) | |
| ] * L; | |
| V = [ | |
| (-L[1] * sin(z[1]) - L[2] * sin(z[1] + z[2])) (-L[2] * sin(z[1] + z[2])) | |
| (L[1] * cos(z[1]) + L[2] * cos(z[1] + z[2])) (L[2] * cos(z[1] + z[2])) | |
| ] * w; | |
| FL = [0, 0]; | |
| tau = [0, 0]; | |
| # Runge-kutta | |
| k = zeros(5, 2); | |
| z1 = z[1]; | |
| z2 = z[2]; | |
| w1 = w[1]; | |
| w2 = w[2]; | |
| l = zeros(5, 2); | |
| for j in 0:4 | |
| if (0 < j) | |
| z1 = z1 + k[j, 1] / 2; | |
| z2 = z2 + k[j, 2] / 2; | |
| w1 = w1 + l[j, 1] / 2; | |
| w2 = w2 + l[j, 2] / 2; | |
| end | |
| k[j + 1, :] = dt * [w1; w2]; | |
| LL = A1 * A2 - A3 ^ 2 * cos(z2) ^ 2; | |
| a11 = A2 / LL; | |
| a12 = -(A2 + A3 * cos(z2)) / LL | |
| a22 = (A1+ A2 + 2.0* A3 * cos(z2)) / LL; | |
| tauL = [ | |
| (-L[1] * sin(z1) - L[2] * sin(z1 + z2)) (-L[2] * sin(z1 + z2)) | |
| (L[1] * cos(z1) + L[2] * cos(z1 + z2)) (L[2] * cos(z1 + z2)) | |
| ] * FL; | |
| u = [ | |
| (tau[1] + tauL[1] + A3 * w[2] * (2.0 * w[1] + w[2]) * sin(z2) - B1 * cos(z1 + z2)) | |
| (tau[2] + tauL[2] + A3 * w[1] * w[1] * sin(z2) - B2 * cos(z1 + z2)) | |
| ]; | |
| l[j + 1, :] = dt * [ | |
| a11 a12 | |
| a12 a22 | |
| ] * u; | |
| end | |
| # Concat | |
| z = z + (k[1, :] + 2.0 * k[2, :] + 2.0 * k[3, :] + k[4, :]) / 6; | |
| w = w + (l[1, :] + 2.0 * l[2, :] + 2.0 * l[3, :] + l[4, :]) / 6; | |
| th[i + 1, :] = z[:]; | |
| thv[i + 1, :] = w[:]; | |
| pos[i + 1, :] = [ | |
| cos(z[1]) 0 | |
| sin(z[1]) 0 | |
| cos(z[1]) cos(z[1] + z[2]) | |
| sin(z[1]) sin(z[1] + z[2]) | |
| ] * L; | |
| end | |
| gr(size = (800, 800)); | |
| p1 = plot(pos[:, 1], pos[:, 2], 0:800, st=:scatter, markercolor=:blue); | |
| p2 = plot(pos[:, 3], pos[:, 4], 0:800, st=:scatter, markercolor=:red); | |
| plot(p1, p2); | |
| end |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment