def __init__(self,
_obj_fun=obj_final_z,
_obj_grad=obj_grad_final_z,
_atm=None,
_min_bank=-np.deg2rad(45.),
_max_bank=np.deg2rad(45.),
x0=-25,
y0=0,
z0=1,
psi0=0):
self.duration = 40
self.num_nodes = 2000 # time discretization
self.interval_value = self.duration / (self.num_nodes - 1)
print('solver: interval_value: {:.3f}s ({:.1f}hz)'.format(
self.interval_value, 1. / self.interval_value))
self.atm = _atm if _atm is not None else atm0()
self._slice_x = slice(0 * self.num_nodes, 1 * self.num_nodes, 1)
self._slice_y = slice(1 * self.num_nodes, 2 * self.num_nodes, 1)
self._slice_z = slice(2 * self.num_nodes, 3 * self.num_nodes, 1)
self._slice_psi = slice(3 * self.num_nodes, 4 * self.num_nodes, 1)
self._slice_phi = slice(4 * self.num_nodes, 5 * self.num_nodes, 1)
# Specify the symbolic instance constraints, i.e. initial and end conditions.
instance_constraints = (x(0.0) - x0, y(0.0) - y0, z(0.) - z0,
psi(0.) - psi0)
#theta(duration) - target_angle,
bounds = {phi(t): (_min_bank, _max_bank), y(t): (-50, 50)}
#bounds = {phi(t): (-0.1, _max_bank)}
# Create an optimization problem.
self.prob = Problem(
lambda _free: _obj_fun(self.num_nodes, self.interval_value, _free),
lambda _free: _obj_grad(self.num_nodes, self.interval_value, _free
),
#eom,
get_eom(self.atm),
state_symbols,
self.num_nodes,
self.interval_value,
known_parameter_map=par_map,
instance_constraints=instance_constraints,
bounds=bounds,
parallel=False)
# q3(0.0),
# q3(duration))
# =============================================================================
# =============================================================================
instance_constraints = (q0(0.0), q0(duration) - target_position, q1(0.0),
q1(duration), q2(0.0), q2(duration) - target_angle,
q3(0.0), q3(duration))
# =============================================================================
# Create an optimization problem.
prob = Problem(obj,
obj_grad,
eom,
state_symbols,
num_nodes,
interval_value,
known_parameter_map=par_map,
instance_constraints=instance_constraints,
bounds={u(t): (-1000.0, 1000.0)})
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
# Make some plots
prob.plot_trajectories(solution)
#prob.plot_constraint_violations(solution)
prob.plot_objective_value()
L2(t): np.concatenate([np.ones(num_nodes / 2),
np.zeros(num_nodes / 2)]),
L3(t): np.concatenate([np.zeros(num_nodes / 2),
np.zeros(num_nodes / 2)]),
L4(t): np.concatenate([np.zeros(num_nodes / 2),
np.zeros(num_nodes / 2)]),
L5(t): np.concatenate([np.zeros(num_nodes / 2),
np.ones(num_nodes / 2)]),
}
# Create an optimization problem.
prob = Problem(obj,
obj_grad,
eom,
state_symbols,
num_nodes,
interval_value,
instance_constraints=instance_constraints,
bounds=bounds,
known_trajectory_map=known_trajectory_map)
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
# Make some plots
prob.plot_trajectories(solution)
plt.show()
def obj_grad(free):
grad = np.zeros_like(free)
grad[:num_nodes] = 2.0 * interval * (free[:num_nodes] - y1_m)
return grad
# Specify the symbolic instance constraints, i.e. initial and end
# conditions.
instance_constraints = (y1(0.0), y2(0.0) - np.pi)
# Create an optimization problem.
prob = Problem(obj,
obj_grad,
eom,
state_symbols,
num_nodes,
interval,
known_parameter_map=par_map,
known_trajectory_map={T(t): time},
instance_constraints=instance_constraints,
integration_method='midpoint')
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
known_msg = "Known value of p = {}".format(np.pi)
identified_msg = "Identified value of p = {}".format(solution[-1])
divider = '=' * max(len(known_msg), len(identified_msg))
grad = np.zeros_like(free)
grad[2 * num_nodes:] = 2.0 * interval_value * free[2 * num_nodes:]
return grad
# Specify the symbolic instance constraints, i.e. initial and end
# conditions.
instance_constraints = (x(0.0), x(duration) - 1.0, y(0.0), y(duration))
# Create an optimization problem.
prob = Problem(obj,
obj_grad,
eom,
state_symbols,
num_nodes,
interval_value,
instance_constraints=instance_constraints,
bounds={
ux(t): (-10, 10),
uy(t): (-10, 10),
y(t): (0, 0)
})
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
# Make some plots
prob.plot_trajectories(solution)
def obj_grad(free):
grad = np.zeros_like(free)
grad[2 * num_nodes:] = 2.0 * interval_value * free[2 * num_nodes:]
return grad # 2T
# Specify the symbolic instance constraints, i.e. initial and end
# conditions.
instance_constraints = (x(0.0), x(duration) - target_position, xdot(0.0),
xdot(duration))
# Create an optimization problem. THIS CAUSES SPYDER ISSUES
prob = Problem(obj,
obj_grad,
eom,
state_symbols,
num_nodes,
interval_value,
instance_constraints=instance_constraints,
bounds={u(t): (-10.0, 30.0)})
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
# Make some plots
prob.plot_trajectories(solution)
prob.plot_constraint_violations(solution)
prob.plot_objective_value()
def obj_grad(free):
grad = np.zeros_like(free)
grad[2 * num_nodes:] = 2.0 * interval_value * free[2 * num_nodes:]
return grad
# Specify the symbolic instance constraints, i.e. initial and end
# conditions.
instance_constraints = (theta(0.0),
theta(duration) - target_angle,
omega(0.0),
omega(duration))
# Create an optimization problem.
prob = Problem(obj, obj_grad, eom, state_symbols, num_nodes, interval_value,
known_parameter_map=par_map,
instance_constraints=instance_constraints,
bounds={T(t): (-2.0, 2.0)})
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
# Make some plots
prob.plot_trajectories(solution)
prob.plot_constraint_violations(solution)
prob.plot_objective_value()
# Display animation
if not building_docs():
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()
def obj_grad(free):
grad = np.zeros_like(free)
grad[:num_nodes] = 2.0 * interval * (free[:num_nodes] - y1_m)
return grad
# Specify the symbolic instance constraints, i.e. initial and end
# conditions.
instance_constraints = (y1(0.0), y2(0.0) - np.pi)
# Create an optimization problem.
prob = Problem(obj, obj_grad,
eom, state_symbols,
num_nodes, interval,
known_parameter_map=par_map,
known_trajectory_map={T(t): time},
instance_constraints=instance_constraints,
integration_method='midpoint')
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
known_msg = "Known value of p = {}".format(np.pi)
identified_msg = "Identified value of p = {}".format(solution[-1])
divider = '=' * max(len(known_msg), len(identified_msg))
print(divider)
class Planner:
def __init__(self,
_obj_fun=obj_final_z,
_obj_grad=obj_grad_final_z,
_atm=None,
_min_bank=-np.deg2rad(45.),
_max_bank=np.deg2rad(45.),
x0=-25,
y0=0,
z0=1,
psi0=0):
self.duration = 40
self.num_nodes = 2000 # time discretization
self.interval_value = self.duration / (self.num_nodes - 1)
print('solver: interval_value: {:.3f}s ({:.1f}hz)'.format(
self.interval_value, 1. / self.interval_value))
self.atm = _atm if _atm is not None else atm0()
self._slice_x = slice(0 * self.num_nodes, 1 * self.num_nodes, 1)
self._slice_y = slice(1 * self.num_nodes, 2 * self.num_nodes, 1)
self._slice_z = slice(2 * self.num_nodes, 3 * self.num_nodes, 1)
self._slice_psi = slice(3 * self.num_nodes, 4 * self.num_nodes, 1)
self._slice_phi = slice(4 * self.num_nodes, 5 * self.num_nodes, 1)
# Specify the symbolic instance constraints, i.e. initial and end conditions.
instance_constraints = (x(0.0) - x0, y(0.0) - y0, z(0.) - z0,
psi(0.) - psi0)
#theta(duration) - target_angle,
bounds = {phi(t): (_min_bank, _max_bank), y(t): (-50, 50)}
#bounds = {phi(t): (-0.1, _max_bank)}
# Create an optimization problem.
self.prob = Problem(
lambda _free: _obj_fun(self.num_nodes, self.interval_value, _free),
lambda _free: _obj_grad(self.num_nodes, self.interval_value, _free
),
#eom,
get_eom(self.atm),
state_symbols,
self.num_nodes,
self.interval_value,
known_parameter_map=par_map,
instance_constraints=instance_constraints,
bounds=bounds,
parallel=False)
def configure(self, tol=1e-8, max_iter=3000):
# https://coin-or.github.io/Ipopt/OPTIONS.html
self.prob.addOption('tol', tol) # default 1e-8
self.prob.addOption('max_iter', max_iter) # default 3000
def run(self):
# Use a random positive initial guess.
initial_guess = np.random.randn(self.prob.num_free)
# Find the optimal solution.
self.solution, info = self.prob.solve(initial_guess)
self.interpret_solution()
def interpret_solution(self):
self.sol_time = np.linspace(0.0, self.duration, num=self.num_nodes)
self.sol_x = self.solution[self._slice_x]
self.sol_y = self.solution[self._slice_y]
self.sol_z = self.solution[self._slice_z]
self.sol_psi = self.solution[self._slice_psi]
self.sol_phi = self.solution[self._slice_phi]
self.sol_v = par_map[v] * np.ones(self.num_nodes)
def save_solution(self, filename):
print('saving {}'.format(filename))
pickle.dump(self.solution, open(filename, "wb"))
def load_solution(self, filename):
print('loading {}'.format(filename))
self.solution = pickle.load(open(filename, "rb"))
self.interpret_solution()
def run_or_load(self, filename, force_run=False):
if force_run or not os.path.exists(filename):
self.run()
self.save_solution(filename)
else:
self.load_solution(filename)
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()
def obj_grad(free):
grad = np.zeros_like(free)
grad[2 * num_nodes:] = 2.0 * interval_value * free[2 * num_nodes:]
return grad
# Specify the symbolic instance constraints, i.e. initial and end
# conditions.
instance_constraints = (theta(0.0),
theta(duration) - target_angle,
omega(0.0),
omega(duration))
# Create an optimization problem.
prob = Problem(obj, obj_grad, eom, state_symbols, num_nodes, interval_value,
known_parameter_map=par_map,
instance_constraints=instance_constraints,
bounds={T(t): (-1.5, 1.5)})
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
# Plot trajectories
time = np.linspace(0.0, duration, num=num_nodes)
angle = solution[:num_nodes]
rate = solution[num_nodes:2 * num_nodes]
torque = solution[2 * num_nodes:]
grad = np.zeros_like(free)
grad[:4 *
num_nodes] = 2.0 * interval * (free[:4 * num_nodes] - x_meas_vec)
return grad
bounds = {}
for g in h.gain_symbols:
bounds[g] = (0.0, 1.0)
prob = Problem(obj,
obj_grad,
h.first_order_implicit(),
h.states(),
num_nodes,
interval,
known_parameter_map=h.closed_loop_par_map,
known_trajectory_map={
h.specified['platform_acceleration']: accel_meas
},
bounds=bounds,
time_symbol=h.time,
integration_method='midpoint')
initial_guess = np.hstack(
(x_meas_vec, (h.gain_scale_factors * h.numerical_gains).flatten()))
initial_guess = np.hstack((x_meas_vec, np.random.random(8)))
initial_guess = np.hstack((x_meas_vec, np.zeros(8)))
initial_guess = np.zeros(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
def obj_grad(free):
grad = np.zeros_like(free)
grad[:4 * num_nodes] = 2.0 * interval * (free[:4 * num_nodes] -
x_meas_vec)
return grad
bounds = {}
for g in h.gain_symbols:
bounds[g] = (0.0, 1.0)
prob = Problem(obj, obj_grad,
h.first_order_implicit(),
h.states(),
num_nodes, interval,
known_parameter_map=h.closed_loop_par_map,
known_trajectory_map={h.specified['platform_acceleration']: accel_meas},
bounds=bounds,
time_symbol=h.time,
integration_method='midpoint')
initial_guess = np.hstack((x_meas_vec, (h.gain_scale_factors *
h.numerical_gains).flatten()))
initial_guess = np.hstack((x_meas_vec, np.random.random(8)))
initial_guess = np.hstack((x_meas_vec, np.zeros(8)))
initial_guess = np.zeros(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
p_sol = solution[-8:]
def obj_grad(free):
grad = np.zeros_like(free)
grad[:num_nodes] = 2.0 * interval * (free[:num_nodes] - y1_m)
return grad
# Specify the symbolic instance constraints, i.e. initial and end
# conditions.
instance_constraints = (y1(0.0), y2(0.0) - np.pi)
# Create an optimization problem.
prob = Problem(obj,
obj_grad,
eom,
state_symbols,
num_nodes,
interval,
known_parameter_map=par_map,
known_trajectory_map={T(t): time},
instance_constraints=instance_constraints,
integration_method='midpoint')
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
known_msg = "Known value of p = {}".format(np.pi)
identified_msg = "Identified value of p = {}".format(solution[-1])
divider = '=' * max(len(known_msg), len(identified_msg))