def main(initial_guess, do_plot=False):
if building_docs:
do_plot = True
# Specify the symbolic equations of motion.
p, t = sym.symbols('p, t')
y1, y2 = [f(t) for f in sym.symbols('y1, y2', cls=sym.Function)]
y = sym.Matrix([y1, y2])
f = sym.Matrix([y2, -p * sym.sin(y1)])
eom = y.diff(t) - f
# Generate some data by integrating the equations of motion.
duration = 50.0
num_nodes = 5000
interval = duration / (num_nodes - 1)
time = np.linspace(0.0, duration, num=num_nodes)
p_val = 10.0
y0 = [np.pi / 6.0, 0.0]
def eval_f(y, t, p):
return np.array([y[1], -p * np.sin(y[0])])
y_meas = odeint(eval_f, y0, time, args=(p_val,))
y1_meas = y_meas[:, 0]
y2_meas = y_meas[:, 1]
# Add measurement noise.
y1_meas += np.random.normal(scale=0.05, size=y1_meas.shape)
y2_meas += np.random.normal(scale=0.1, size=y2_meas.shape)
# Setup the optimization problem to minimize the error in the simulated
# angle and the measured angle.
def obj(free):
"""Minimize the error in the angle, y1."""
return interval * np.sum((y1_meas - free[:num_nodes])**2)
def obj_grad(free):
grad = np.zeros_like(free)
grad[:num_nodes] = 2.0 * interval * (free[:num_nodes] - y1_meas)
return grad
# The midpoint integration method is preferable to the backward Euler
# method because no artificial damping is introduced.
prob = Problem(obj, obj_grad, eom, (y1, y2), num_nodes, interval,
integration_method='midpoint')
num_states = len(y)
if initial_guess == 'zero':
print('Using all zeros for the initial guess.')
# All zeros.
initial_guess = np.zeros(num_states * num_nodes + 1)
elif initial_guess == 'randompar':
print(('Using all zeros for the trajectories initial guess and a '
'random positive value for the parameter.'))
# Zeros for the state trajectories and a random positive value for
# the parameter.
initial_guess = np.hstack((np.zeros(num_states * num_nodes), 50.0 *
np.random.random(1)))
elif initial_guess == 'random':
print('Using random values for the initial guess.')
# Random values for all unknowns.
initial_guess = np.hstack((np.random.normal(scale=5.0,
size=num_states *
num_nodes), 50.0 *
np.random.random(1)))
elif initial_guess == 'sysid':
print(('Using noisy measurements for the trajectory initial guess and '
'a random positive value for the parameter.'))
# Give noisy measurements as the initial state guess and a random
# positive values as the parameter guess.
initial_guess = np.hstack((y1_meas, y2_meas, 100.0 *
np.random.random(1)))
elif initial_guess == 'known':
print('Using the known solution as the initial guess.')
# Known solution as initial guess.
initial_guess = np.hstack((y1_meas, y2_meas, 10.0))
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
p_sol = solution[-1]
# Print the result.
known_msg = "Known value of p = {}".format(p_val)
guess_msg = "Initial guess for p = {}".format(initial_guess[-1])
identified_msg = "Identified value of p = {}".format(p_sol)
divider = '=' * max(len(known_msg), len(identified_msg))
print(divider)
print(known_msg)
print(guess_msg)
print(identified_msg)
print(divider)
# Simulate with the identified parameter.
y_sim = odeint(eval_f, y0, time, args=(p_sol,))
y1_sim = y_sim[:, 0]
y2_sim = y_sim[:, 1]
if do_plot:
# Plot results
fig_y1, axes_y1 = plt.subplots(3, 1)
legend = ['measured', 'initial guess',
'direct collocation solution', 'identified simulated']
axes_y1[0].plot(time, y1_meas, '.k',
time, initial_guess[:num_nodes], '.b',
time, solution[:num_nodes], '.r',
time, y1_sim, 'g')
axes_y1[0].set_xlabel('Time [s]')
axes_y1[0].set_ylabel('y1 [rad]')
axes_y1[0].legend(legend)
axes_y1[1].set_title('Initial Guess Constraint Violations')
axes_y1[1].plot(prob.con(initial_guess)[:num_nodes - 1])
axes_y1[2].set_title('Solution Constraint Violations')
axes_y1[2].plot(prob.con(solution)[:num_nodes - 1])
plt.tight_layout()
fig_y2, axes_y2 = plt.subplots(3, 1)
axes_y2[0].plot(time, y2_meas, '.k',
time, initial_guess[num_nodes:-1], '.b',
time, solution[num_nodes:-1], '.r',
time, y2_sim, 'g')
axes_y2[0].set_xlabel('Time [s]')
axes_y2[0].set_ylabel('y2 [rad]')
axes_y2[0].legend(legend)
axes_y2[1].set_title('Initial Guess Constraint Violations')
axes_y2[1].plot(prob.con(initial_guess)[num_nodes - 1:])
axes_y2[2].set_title('Solution Constraint Violations')
axes_y2[2].plot(prob.con(solution)[num_nodes - 1:])
plt.tight_layout()
plt.show()
print(divider)
# Plot results
fig_y1, axes_y1 = plt.subplots(3, 1)
legend = ['measured', 'initial guess', 'direct collocation solution']
axes_y1[0].plot(time, y1_m, '.k',
time, initial_guess[:num_nodes], '.b',
time, solution[:num_nodes], '.r')
axes_y1[0].set_xlabel('Time [s]')
axes_y1[0].set_ylabel('y1')
axes_y1[0].legend(legend)
axes_y1[1].set_title('Initial Guess Constraint Violations')
axes_y1[1].plot(prob.con(initial_guess)[:num_nodes - 1])
axes_y1[2].set_title('Solution Constraint Violations')
axes_y1[2].plot(prob.con(solution)[:num_nodes - 1])
plt.tight_layout()
fig_y2, axes_y2 = plt.subplots(3, 1)
axes_y2[0].plot(time, y2_m, '.k',
time, initial_guess[num_nodes:-1], '.b',
time, solution[num_nodes:-1], '.r')
axes_y2[0].set_xlabel('Time [s]')
axes_y2[0].set_ylabel('y2')
axes_y2[0].legend(legend)
axes_y2[1].set_title('Initial Guess Constraint Violations')
angle = solution[:num_nodes]
rate = solution[num_nodes:2 * num_nodes]
torque = solution[2 * num_nodes:]
fig, axes = plt.subplots(3)
axes[0].set_title('State and Control Trajectories')
axes[0].plot(time, angle)
axes[0].set_ylabel('Angle [rad]')
axes[1].plot(time, rate)
axes[1].set_ylabel('Angular Rate [rad/s]')
axes[2].plot(time, torque)
axes[2].set_ylabel('Torque [Nm]')
axes[2].set_xlabel('Time [S]')
# Plot constraint violations
con_violations = prob.con(solution)
con_nodes = range(2, num_nodes + 1)
N = len(con_nodes)
fig, axes = plt.subplots(3)
axes[0].set_title('Constraint Violations')
axes[0].plot(con_nodes, con_violations[:N])
axes[0].set_ylabel('Angle [rad]')
axes[1].plot(con_nodes, con_violations[N:2 * N])
axes[1].set_ylabel('Angular Rate [rad/s]')
axes[1].set_xlabel('Node Number')
axes[2].plot(con_violations[2 * N:])
axes[2].set_ylabel('Instance')
# Plot objective value
fig, ax = plt.subplots(1)
ax.set_title('Objective Value')
def main(initial_guess, do_plot=False):
# Specify the symbolic equations of motion.
p, t = sym.symbols('p, t')
y1, y2 = [f(t) for f in sym.symbols('y1, y2', cls=sym.Function)]
y = sym.Matrix([y1, y2])
f = sym.Matrix([y2, -p * sym.sin(y1)])
eom = y.diff(t) - f
# Generate some data by integrating the equations of motion.
duration = 50.0
num_nodes = 5000
interval = duration / (num_nodes - 1)
time = np.linspace(0.0, duration, num=num_nodes)
p_val = 10.0
y0 = [np.pi / 6.0, 0.0]
def eval_f(y, t, p):
return np.array([y[1], -p * np.sin(y[0])])
y_meas = odeint(eval_f, y0, time, args=(p_val,))
y1_meas = y_meas[:, 0]
y2_meas = y_meas[:, 1]
# Add measurement noise.
y1_meas += np.random.normal(scale=0.05, size=y1_meas.shape)
y2_meas += np.random.normal(scale=0.1, size=y2_meas.shape)
# Setup the optimization problem to minimize the error in the simulated
# angle and the measured angle.
def obj(free):
"""Minimize the error in the angle, y1."""
return interval * np.sum((y1_meas - free[:num_nodes])**2)
def obj_grad(free):
grad = np.zeros_like(free)
grad[:num_nodes] = 2.0 * interval * (free[:num_nodes] - y1_meas)
return grad
# The midpoint integration method is preferable to the backward Euler
# method because no artificial damping is introduced.
prob = Problem(obj, obj_grad, eom, (y1, y2), num_nodes, interval,
integration_method='midpoint')
# Set some IPOPT options.
prob.addOption('linear_solver', 'ma57')
num_states = len(y)
if initial_guess == 'zero':
# All zeros.
initial_guess = np.zeros(num_states * num_nodes + 1)
elif initial_guess == 'randompar':
# Zeros for the state trajectories and a random positive value for
# the parameter.
initial_guess = np.hstack((np.zeros(num_states * num_nodes), 50.0 *
np.random.random(1)))
elif initial_guess == 'random':
# Random values for all unknowns.
initial_guess = np.hstack((np.random.normal(scale=5.0,
size=num_states *
num_nodes), 50.0 *
np.random.random(1)))
elif initial_guess == 'sysid':
# Give noisy measurements as the initial state guess and a random
# positive values as the parameter guess.
initial_guess = np.hstack((y1_meas, y2_meas, 100.0 *
np.random.random(1)))
elif initial_guess == 'known':
# Known solution as initial guess.
initial_guess = np.hstack((y1_meas, y2_meas, 10.0))
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
p_sol = solution[-1]
# Print the result.
known_msg = "Known value of p = {}".format(p_val)
guess_msg = "Initial guess for p = {}".format(initial_guess[-1])
identified_msg = "Identified value of p = {}".format(p_sol)
divider = '=' * max(len(known_msg), len(identified_msg))
print(divider)
print(known_msg)
print(guess_msg)
print(identified_msg)
print(divider)
# Simulate with the identified parameter.
y_sim = odeint(eval_f, y0, time, args=(p_sol,))
y1_sim = y_sim[:, 0]
y2_sim = y_sim[:, 1]
if do_plot:
# Plot results
fig_y1, axes_y1 = plt.subplots(3, 1)
legend = ['measured', 'initial guess',
'direct collocation solution', 'identified simulated']
axes_y1[0].plot(time, y1_meas, '.k',
time, initial_guess[:num_nodes], '.b',
time, solution[:num_nodes], '.r',
time, y1_sim, 'g')
axes_y1[0].set_xlabel('Time [s]')
axes_y1[0].set_ylabel('y1 [rad]')
axes_y1[0].legend(legend)
axes_y1[1].set_title('Initial Guess Constraint Violations')
axes_y1[1].plot(prob.con(initial_guess)[:num_nodes - 1])
axes_y1[2].set_title('Solution Constraint Violations')
axes_y1[2].plot(prob.con(solution)[:num_nodes - 1])
plt.tight_layout()
fig_y2, axes_y2 = plt.subplots(3, 1)
axes_y2[0].plot(time, y2_meas, '.k',
time, initial_guess[num_nodes:-1], '.b',
time, solution[num_nodes:-1], '.r',
time, y2_sim, 'g')
axes_y2[0].set_xlabel('Time [s]')
axes_y2[0].set_ylabel('y2 [rad]')
axes_y2[0].legend(legend)
axes_y2[1].set_title('Initial Guess Constraint Violations')
axes_y2[1].plot(prob.con(initial_guess)[num_nodes - 1:])
axes_y2[2].set_title('Solution Constraint Violations')
axes_y2[2].plot(prob.con(solution)[num_nodes - 1:])
plt.tight_layout()
plt.show()
print(identified_msg)
print(divider)
# Plot results
fig_y1, axes_y1 = plt.subplots(3, 1)
legend = ['measured', 'initial guess', 'direct collocation solution']
axes_y1[0].plot(time, y1_m, '.k', time, initial_guess[:num_nodes], '.b', time,
solution[:num_nodes], '.r')
axes_y1[0].set_xlabel('Time [s]')
axes_y1[0].set_ylabel('y1')
axes_y1[0].legend(legend)
axes_y1[1].set_title('Initial Guess Constraint Violations')
axes_y1[1].plot(prob.con(initial_guess)[:num_nodes - 1])
axes_y1[2].set_title('Solution Constraint Violations')
axes_y1[2].plot(prob.con(solution)[:num_nodes - 1])
plt.tight_layout()
fig_y2, axes_y2 = plt.subplots(3, 1)
axes_y2[0].plot(time, y2_m, '.k', time, initial_guess[num_nodes:-1], '.b',
time, solution[num_nodes:-1], '.r')
axes_y2[0].set_xlabel('Time [s]')
axes_y2[0].set_ylabel('y2')
axes_y2[0].legend(legend)
axes_y2[1].set_title('Initial Guess Constraint Violations')
axes_y2[1].plot(prob.con(initial_guess)[num_nodes - 1:])