Simulation of 2 link robot arm

2 link robot arm.jl

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