I am trying to plot the changes in body angle using arrow in plot. I set the initial body angle as 45 degrees. I want to make the blue square and the arrow turn according to the change in angle. q is the position of the robot at every frame, and ang should be the angle at that frame. Rotation in anti-clockwise direction is set as positive.
begin
# The robot's starting position and velocity
q = [1.0, 0.0]
v = [-2.0, 2.0]
ang = 45
Δt = 0.1
qs_x = []
qs_y = []
as_x = []
as_y = []
angs = []
anim = @animate for i in 1:80 # This determies the number of MPC to be run
# Plot the current position
# Attitude of the vehicle is not considered
plot([q[1]], [q[2]], marker=(:rect, 10), xlim=(-2, 2), ylim=(-2, 2))
push!(qs_x,q[1])
push!(qs_y,q[2])
plot!([q[1]], [q[2]], marker=(:cross, 18, :grey))
# Run the MPC control optimization
q_plan, v_plan, u_plan, ang_plan = run_mpc(q, v, ang)
# Draw the planned future states from the MPC optimization
plot!(q_plan[1, :], q_plan[2, :], linewidth=5, arrow=true, c=:orange)
# Save Acceleration & Angle Data to csv
u = u_plan[:, 1]
global ang = ang_plan[1]
push!(as_x, u[1])
push!(as_y, u[2])
push!(angs, ang_plan[1])
# Apply the planned acceleration and simulate one step in time
global v += u * Δt
global q += v * Δt
end
gif(anim, "~/Downloads/NLmpc_angle.gif", fps=60)
end