def inv_map(self, sol: Solution) -> Solution:
qinv = np.ones_like(sol.t)
pinv = np.ones_like(sol.t)
for ii, t in enumerate(sol.t):
qinv[ii] = self.fn_q_inv(sol.y[ii], sol.lam[ii], sol.q[ii], sol.p,
sol.k)
pinv[ii] = self.fn_p_inv(sol.y[ii], sol.lam[ii], sol.p, sol.k)
state = pinv
qval = qinv
if self.remove_parameter_dict['location'] == 'states':
sol.y = np.column_stack(
(sol.y[:, :self.remove_parameter_dict['index']], state,
sol.y[:, self.remove_parameter_dict['index']:]))
elif self.remove_parameter_dict['location'] == 'costates':
sol.lam = np.column_stack(
(sol.lam[:, :self.remove_parameter_dict['index']], state,
sol.lam[:, self.remove_parameter_dict['index']:]))
if self.remove_symmetry_dict['location'] == 'states':
sol.y = np.column_stack(
(sol.y[:, :self.remove_symmetry_dict['index']], qval,
sol.y[:, self.remove_symmetry_dict['index']:]))
elif self.remove_symmetry_dict['location'] == 'costates':
sol.lam = np.column_stack(
(sol.lam[:, :self.remove_symmetry_dict['index']], qval,
sol.lam[:, self.remove_symmetry_dict['index']:]))
sol.q = np.delete(sol.q, np.s_[-1], axis=1)
sol.p = sol.p[:-1]
return sol
def map(self, sol: Solution) -> Solution:
cval = self.fn_p(sol.y[0], sol.lam[0], sol.p, sol.k)
qval = np.ones_like(sol.t)
sol.p = np.hstack((sol.p, cval))
for ii, t in enumerate(sol.t):
qval[ii] = self.fn_q(sol.y[ii], sol.lam[ii], sol.p, sol.k)
if self.remove_parameter_dict['location'] == 'states':
sol.y = np.delete(sol.y,
np.s_[self.remove_parameter_dict['index']],
axis=1)
elif self.remove_parameter_dict['location'] == 'costates':
sol.lam = np.delete(sol.lam,
np.s_[self.remove_parameter_dict['index']],
axis=1)
if self.remove_symmetry_dict['location'] == 'states':
sol.y = np.delete(sol.y,
np.s_[self.remove_symmetry_dict['index']],
axis=1)
elif self.remove_symmetry_dict['location'] == 'costates':
sol.lam = np.delete(sol.lam,
np.s_[self.remove_symmetry_dict['index']],
axis=1)
sol.q = np.column_stack((sol.q, qval))
return sol
def map(self,
sol: Solution,
control_costate: Union[float, np.ndarray] = 0.) -> Solution:
idx_u_list = []
for idx_u, (idx_y,
u) in enumerate(sorted(zip(self.control_idxs, sol.u.T))):
sol.y = np.insert(sol.y, idx_y, u, axis=1)
if isinstance(control_costate, Iterable):
if not isinstance(control_costate, np.ndarray):
control_costate = np.array(control_costate)
costate_insert = control_costate[idx_u] * np.ones_like(sol.t)
else:
costate_insert = control_costate * np.ones_like(sol.t)
sol.lam = np.insert(sol.lam, -1, costate_insert, axis=1)
if len(sol.lam_u) == 0:
sol.lam_u = np.array([costate_insert])
else:
sol.lam_u = np.insert(sol.lam_u, -1, costate_insert, axis=1)
idx_u_list.append(idx_u)
sol.u = np.delete(sol.u, idx_u_list, axis=1)
return sol
def inv_map(self, sol: Solution, retain_dual=True) -> Solution:
if not retain_dual:
sol.lam = empty_array
sol.nu = empty_array
sol.cost = self.compute_cost(sol.t, sol.y, sol.q, sol.u, sol.p, sol.k)
return sol
def inv_map(self, sol: Solution) -> Solution:
if self.ind_state_idx is None:
self.ind_state_idx = np.shape(sol.y)[1] - 1
sol.t = sol.y[:, self.ind_state_idx]
sol.y = np.delete(sol.y, self.ind_state_idx, axis=1)
sol.lam_t = sol.lam[:, self.ind_state_idx]
sol.lam = np.delete(sol.lam, self.ind_state_idx, axis=1)
return sol
def map(self, sol: Solution) -> Solution:
if self.ind_state_idx is None:
self.ind_state_idx = np.shape(sol.y)[1]
if len(sol.lam_t) == 0:
sol.lam_t = np.zeros_like(sol.t)
sol.y = np.insert(sol.y, self.ind_state_idx, sol.t, axis=1)
sol.lam = np.insert(sol.lam, self.ind_state_idx, sol.lam_t, axis=1)
return sol
def test_composable_functors(method):
problem = Problem()
problem.independent('t', 's')
problem.state('x', 'v*cos(theta)', 'm')
problem.state('y', 'v*sin(theta)', 'm')
problem.state('v', 'g*sin(theta)', 'm/s')
problem.constant_of_motion('c1', 'lamX', 's/m')
problem.constant_of_motion('c2', 'lamY', 's/m')
problem.control('theta', 'rad')
problem.constant('g', -9.81, 'm/s^2')
problem.constant('x_f', 1, 'm')
problem.constant('y_f', -1, 'm')
problem.path_cost('1', '1')
problem.initial_constraint('x', 'm')
problem.initial_constraint('y', 'm')
problem.initial_constraint('v', 'm')
problem.terminal_constraint('x - x_f', 'm')
problem.terminal_constraint('y - y_f', 'm')
problem.scale(m='y', s='y/v', kg=1, rad=1, nd=1)
preprocessor = make_preprocessor()
indirect_method = make_indirect_method(problem, method=method)
bvp = indirect_method(preprocessor(problem))
mapper = bvp.map_sol
mapper_inv = bvp.inv_map_sol
gamma = Trajectory()
gamma.t = np.linspace(0, 1, num=10)
gamma.y = np.vstack([np.linspace(0, 0, num=10) for _ in range(3)]).T
gamma.lam = np.vstack([np.linspace(-1, -1, num=10) for _ in range(3)]).T
gamma.u = -np.pi / 2 * np.ones((10, 1))
gamma.k = np.array([-9.81, 1, -1])
g1 = mapper(copy.deepcopy(gamma))
g2 = mapper_inv(copy.deepcopy(g1))
assert g2.y.shape == gamma.y.shape
assert (g2.y - gamma.y < tol).all()
assert g2.q.shape == gamma.q.shape
assert (g2.q - gamma.q < tol).all()
assert g2.lam.shape == gamma.lam.shape
assert (g2.lam - gamma.lam < tol).all()
assert g2.u.shape == gamma.u.shape
assert (g2.u - gamma.u < tol).all()
assert g2.t.size == gamma.t.size
assert (g2.t - gamma.t < tol).all()
assert g2.p.size == gamma.p.size
assert (g2.p - gamma.p < tol).all()
assert g2.nu.size == gamma.nu.size
assert (g2.nu - gamma.nu < tol).all()
def inv_map(self, sol: Solution) -> Solution:
sol.lam = sol.y[:, self.costate_idxs]
sol.y = np.delete(sol.y, self.costate_idxs, axis=1)
sol.nu = np.delete(sol.nu, self.constraint_adjoints_idxs)
return sol
def inv_map(self, sol: Solution) -> Solution:
sol.u = sol.y[:, self.control_idxs]
sol.y = np.delete(sol.y, self.control_idxs, axis=1)
sol.lam = np.delete(sol.lam, self.control_idxs, axis=1)
return sol
