MATLABで解く移動ロボットの問題[勝手に回答]

わがままに他のサイトの質問を勝手に回答するシリーズ。今回は MATLABで解く移動ロボットの問題。 teratailの質問です。

といっても，私は貧乏人なのでMatlabは買えないのでJulia言語で解いてみます。

問題

MATLABで解く移動ロボットの問題 を見て下さい。

実装例

よく分かりませんが，こんな感じですかね。

using Plots

rot₀(θ) = [cos(θ) -sin(θ) 0.0; sin(θ) cos(θ) 0.0; 0.0 0.0 1.0]
move₀(x, y) = [1.0 0.0 x; 0.0 1.0 y; 0.0 0.0 1.0]

function rot_matrix₀(Δθ, mat)
    m1 = mat[1:2, 1]
    x₀ ,y₀ = m1
    move₀(x₀, y₀) * rot₀(Δθ) * move₀(-x₀, -y₀) * mat
end

function move_forward_matrix₀(r, mat)
    m1, m2  = mat[1:2, 1], mat[1:2, 2]
    Δx₀, Δy₀ = (m2 - m1) .* r
    move₀(Δx₀, Δy₀) * mat
end

function plot_robot(mat, ano_txt)
    _makeshape = if isdefined(Plots, :PlotsBase)
        PlotsBase.Shapes.makeshape
    else
        Plots.makeshape
    end
    x₀, y₀ = mat[1:2, 1]
    x₁, y₁ = mat[1:2, 2]
        
    plt = plot(;frame_style=:box, grid=false, lims=(-10, 10),
               size=(600, 600), annotation=(0, 9, ano_txt),
               xguide="x-axis [a.u.]", yguide="y-axis [a.u.]")
    plot!(plt, translate(_makeshape(20), x₀, y₀), fillcolor=:cyan,
          label="")
    plot!(plt, [(x₀, y₀), (x₁, y₁)], arrow=:head, linewidth=2,
          linecolor=:blue, label="")
    return plt
end

function make_robot_animation()
    my_animation = Animation()
    robot_mat = [0.0 0.0;
                 0.0 1.0;
                 1.0 1.0]
    for i in 1:10 # ダミーフレーム
        frame(my_animation, plot_robot(robot_mat, "frame:  0"))
    end
    for k in 1:99
        robot_mat = rot_matrix₀(π / 3, robot_mat)
        frame(my_animation, plot_robot(robot_mat, "frame:$(lpad(k, 3))"))
        robot_mat = move_forward_matrix₀(1.0, robot_mat)
        frame(my_animation, plot_robot(robot_mat, "frame:$(lpad(k, 3))"))
    end
    for i in 1:9 # ダミーフレーム
        frame(my_animation, plot_robot(robot_mat, "frame:$(lpad(99, 3))"))
    end
    gif(my_animation, "robot.gif"; fps=5, loop=0)
end