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], [q], marker=(:rect, 10), xlim=(-2, 2), ylim=(-2, 2)) push!(qs_x,q) push!(qs_y,q) plot!([q], [q], 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 push!(as_x, u) push!(as_y, u) push!(angs, ang_plan) # 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