using PyPlot
# Create evenly spaced time samples from t=0 to t=10.
t_min = 0.0
t_max = 10.0
num_samples = 101
dt = (t_max - t_min) / (num_samples - 1)
t = linspace(t_min, t_max, num_samples)
println(t)
println(dt)
0.0:0.1:10.0
# Create a goal trajectory.
num_dims = 2
G = [ (sin.(2*t/pi) + t/pi) ((t/pi).^2) ]
scatter(G[:,2],G[:,1],c=t,cmap="spring");
plt[:gca]()[:set_aspect]("equal");
# Create a candidate position trajectory, useful as a debugging tool.
Q = [ (sin.(2*t/pi) + 2*t/pi) (t) ]
scatter(G[:,2],G[:,1],c=t,cmap="spring");
scatter(Q[:,2],Q[:,1],c=t,cmap="summer");
plt[:gca]()[:set_aspect]("equal");
# Create a candidate thrust trajectory, again as a debugging tool.
# Note that our candidate position and thrust trajectories don't
# match each other (i.e., executing this sequence of thrusts
# would not actually produce this sequence of positions). But
# this doesn't matter for now. We can still use this candidate
# trajectory to verify the correctness of our algebraic
# manipulations that we will perform on the objective.
U = ones(size(Q));
# Evaluate our objective, expressed in the most straightforward way possible.
alpha = 1.0
E = 0.0
for i = 1:num_samples
g_i = G[i,:]
q_i = Q[i,:]
u_i = U[i,:]
r_i = ((q_i - g_i)'*(q_i - g_i) + alpha*u_i'*u_i)*dt
E = E + r_i
end
println(E)
86.53018113731959
# As a fun example of the kind of debugging we can do, we can verify
# that our objective can also be written as follows,
#
# trace( (Q-G)(Q-G)' )dt + trace( UU' )dt
#
# Working with this form is not neccesary to complete the homework, it
# is just a fun example of the kind of debugging we can do with our
# candidate trajectory.
println(trace((Q - G)*(Q - G)')*dt + alpha*trace(U*U')*dt)
86.53018113731959
# Create a candidate velocity trajectory. Again, this velocity
# trajectory won't match our position and thrust trajectory.
# But again, this doesn't matter for the purposes of debugging
# our objective function.
Q_dot = zeros(size(Q))
# Create a single stacked vector of all the variables from our
# candidate trajectory.
X = [ Q Q_dot U ]
x = X'[:]
println(X[1,:])
println(x[1:6])
[0.0, 0.0, 0.0, 0.0, 1.0, 1.0] [0.0, 0.0, 0.0, 0.0, 1.0, 1.0]
# Choosing an arbitrary incorrect A and b.
num_rows = 10
A_wrong = randn(num_rows,size(x)[1])
b_wrong = ones(num_rows)
# We can verify that this is the wrong choice of A and b,
# because we don't get the same objective value as we did
# previously.
println(norm(A_wrong*x - b_wrong)^2)
58842.90711475393