def new_model( # Initial conditions x_b0=0, y_b0=0, z_b0=0, vx_b0=10, vy_b0=5, vz_b0=15, x_c0=20, y_c0=5, vx_c0=0, vy_c0=0, # Initial covariance S0=ca.diagcat([0.1, 0.1, 0, 0.1, 0.1, 0, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2]) * 0.25, # Hypercovariance weight L0_weight=1e-5, # Mass of the ball mass=0.15, # Discretization time step dt=0.1, # Number of Runge-Kutta integration intervals per time step n_rk=10, # Reaction time (in units of dt) n_delay=1, # System noise weight M_weight=1e-3, # Observation noise N_min=1e-2, # when looking directly at the ball N_max=1e0, # when the ball is 90 degrees from the gaze direction # Final cost: w_cl * distance_between_ball_and_catcher w_cl=1e3, # Running cost on controls: u.T * R * u R=1e0 * ca.diagcat([1e1, 1e0, 1e0, 1e-1]), # Final cost of uncertainty: w_Sl * tr(S) w_Sl=1e3, # Running cost of uncertainty: w_S * tr(S) w_S=1e2, # Control limits F_c1=7.5, F_c2=2.5, w_max=2 * ca.pi, psi_max=0.8 * ca.pi/2, ): phi0 = ca.arctan2(y_b0-y_c0, x_b0-x_c0) # direction towards the ball if phi0 < 0: phi0 += 2 * ca.pi psi0 = 0 # Initial mean m0 = ca.DMatrix([x_b0, y_b0, z_b0, vx_b0, vy_b0, vz_b0, x_c0, y_c0, vx_c0, vy_c0, phi0, psi0]) # Hypercovariance L0 = ca.DMatrix.eye(m0.size()) * L0_weight # System noise matrix M = ca.DMatrix.eye(m0.size()) * M_weight # Catcher dynamics is less noisy M[-6:, -6:] = ca.DMatrix.eye(6) * 1e-5 return Model((m0, S0, L0), dt, n_rk, n_delay, (M, N_min, N_max), (w_cl, R, w_Sl, w_S), (F_c1, F_c2, w_max, psi_max))
def set_initial_condition_as_uncertain(self, par, mean, cov, distribution='normal'): par = vertcat(par) mean = vertcat(mean) cov = vertcat(cov) if not par.numel() == mean.numel(): raise ValueError('Size of "par" and "mean" differ. par.numel()={} ' 'and mean.numel()={}'.format( par.numel(), mean.numel())) if not cov.shape == (mean.numel(), mean.numel()): raise ValueError( 'The input "cov" is not a square matrix of same size as "mean". ' 'cov.shape={} and mean.numel()={}'.format( cov.shape, mean.numel())) self.uncertain_initial_conditions = vertcat( self.uncertain_initial_conditions, par) self.uncertain_initial_conditions_mean = vertcat( self.uncertain_initial_conditions_mean, mean) self.uncertain_initial_conditions_distribution = self.uncertain_initial_conditions_distribution + [ distribution ] * par.numel() if cov.numel() > 0 and self.uncertain_initial_conditions_cov.numel( ) > 0: self.uncertain_initial_conditions_cov = diagcat( self.uncertain_initial_conditions_cov, cov) elif cov.numel() > 0: self.uncertain_initial_conditions_cov = cov else: self.uncertain_initial_conditions_cov = self.uncertain_initial_conditions_cov
def set_parameter_as_uncertain_parameter(self, par, mean, cov, distribution='normal'): par = vertcat(par) mean = vertcat(mean) cov = vertcat(cov) if not par.numel() == mean.numel(): raise ValueError('Size of "par" and "mean" differ. par.numel()={} ' 'and mean.numel()={}'.format( par.numel(), mean.numel())) if not cov.shape == (mean.numel(), mean.numel()): raise ValueError( 'The input "cov" is not a square matrix of same size as "mean". ' 'cov.shape={} and mean.numel()={}'.format( cov.shape, mean.numel())) self.p_unc = vertcat(self.p_unc, par) self.p_unc_mean = vertcat(self.p_unc_mean, mean) # TODO: Work around for casadi diagcat bug, remove when patched. if self.p_unc_cov.numel() == 0: self.p_unc_cov = cov else: self.p_unc_cov = diagcat(self.p_unc_cov, cov) self.p_unc_dist = self.p_unc_dist + [distribution] * par.numel()
def set_augmented_discrete_system(self): """ Pendulum dynamics with integral action. :param x: state :type x: casadi.DM :param u: control input :type u: casadi.DM """ # Grab equilibrium dynamics Ad_eq = self.Ad(self.x_eq, self.u_eq, self.w) Bd_eq = self.Bd(self.x_eq, self.u_eq, self.w) Bw_eq = self.Bw(self.x_eq, self.u_eq, self.w) # Instantiate augmented system self.Ad_i = ca.DM.zeros(5,5) self.Bd_i = ca.DM.zeros(5,1) self.Bw_i = ca.DM.zeros(5,1) self.Cd_i = ca.DM.zeros(1,5) self.R_i = ca.DM.zeros(5,1) # Populate matrices self.Ad_i = ca.diagcat(Ad_eq, ca.DM(1)) self.Ad_i[4,0:4] = -self.dt * self.Cd_eq self.Bd_i = ca.vertcat(Bd_eq, ca.DM(0)) self.Bw_i = ca.vertcat(Bw_eq, ca.DM(0)) self.Cd_i = ca.horzcat(self.Cd_eq, ca.DM(1)) self.R_i[4,0] = self.dt
def _sample_parameters(self): n_samples = self.n_samples n_uncertain = self.n_uncertain mean = vertcat(self.socp.p_unc_mean, self.socp.uncertain_initial_conditions_mean) if self.socp.n_p_unc > 0 and self.socp.n_uncertain_initial_condition > 0: covariance = diagcat(self.socp.p_unc_cov, self.socp.uncertain_initial_conditions_cov) elif self.socp.n_p_unc > 0: covariance = self.socp.p_unc_cov elif self.socp.n_uncertain_initial_condition > 0: covariance = self.socp.uncertain_initial_conditions_cov else: raise ValueError("No uncertainties found n_p_unc = {}, " "n_uncertain_initial_condition={}".format( self.socp.n_p_unc, self.socp.n_uncertain_initial_condition)) dist = self.socp.p_unc_dist + self.socp.uncertain_initial_conditions_distribution for d in dist: if not d == 'normal': raise NotImplementedError( 'Distribution "{}" not implemented, only "normal" is available.' .format(d)) sampled_epsilon = sample_parameter_normal_distribution_with_sobol( DM.zeros(mean.shape), DM.eye(covariance.shape[0]), n_samples) sampled_parameters = SX.zeros(n_uncertain, n_samples) for s in range(n_samples): sampled_parameters[:, s] = mean + mtimes(sampled_epsilon[:, s].T, chol(covariance)).T return sampled_parameters
def diagcat(inputobj): return ca.diagcat(inputobj)
vy_b0 = 4 vz_b0 = 15 x_c0 = 22 y_c0 = 7 vx_c0 = vy_c0 = 0 phi0 = ca.arctan2(y_b0-y_c0, x_b0-x_c0) # direction towards the ball if phi0 < 0: phi0 += 2 * ca.pi psi0 = 0 # Initial mean m0 = ca.DMatrix([x_b0, y_b0, z_b0, vx_b0, vy_b0, vz_b0, x_c0, y_c0, vx_c0, vy_c0, phi0, psi0]) # Initial covariance S0 = ca.diagcat([0.2, 0.2, 0, 0.5, 0.5, 0, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2]) * 0.25 # Hypercovariance L0 = ca.DMatrix.eye(m0.size()) * 1e-5 # Discretization step dt = 0.1 # Number of Runge-Kutta integration intervals per time step n_rk = 10 # Reaction time (in units of dt) n_delay = 2 # System noise matrix M = ca.DMatrix.eye(m0.size()) * 1e-3 M[-6:, -6:] = ca.DMatrix.eye(6) * 1e-5 # catcher's dynamics is less noisy # Observation noise N_min = 1e-2 # when looking directly at the ball N_max = 1e0 # when the ball is 90 degrees from the gaze direction # Final cost: w_cl * distance_between_ball_and_catcher
def diagcat(inputobj): return ca.diagcat(*inputobj)
F = ca.SXFunction('Discrete dynamics', [state, control, dt_sym], [state_next], op) Fj_x = F.jacobian('state') Fj_u = F.jacobian('control') F_xx = ca.SXFunction('F_xx', [state, control, dt_sym], [ca.jacobian(F.jac('state')[i, :].T, state) for i in range(nx)]) F_uu = ca.SXFunction('F_uu', [state, control, dt_sym], [ca.jacobian(F.jac('control')[i, :].T, control) for i in range(nx)]) F_ux = ca.SXFunction('F_ux', [state, control, dt_sym], [ca.jacobian(F.jac('control')[i, :].T, state) for i in range(nx)]) # Cost functions Qf = ca.diagcat([1., 1., 0.]) final_cost = 0.5 * ca.mul([state.cat.T, Qf, state.cat]) op = {'input_scheme': ['state'], 'output_scheme': ['cost']} lf = ca.SXFunction('Final cost', [state], [final_cost], op) R = ca.DMatrix.eye(nu) * dt * 1e-2 cost = 0.5 * ca.mul([control.cat.T, R, control.cat]) op = {'input_scheme': ['state', 'control', 'dt'], 'output_scheme': ['cost']} l = ca.SXFunction('Running cost', [state, control, dt_sym], [cost], op) # Quadratic expansions of costs lfg = lf.gradient('state') # lf_x lfh = lf.hessian('state') # lf_xx
# Extended belief (mu, Sigma, Lambda) for plotting ext_belief = cat.struct_symSX([ cat.entry('m',struct=state), cat.entry('S',shapestruct=(state,state)), cat.entry('L',shapestruct=(state,state)) ]) # %% ========================================================================= # Initial conditions # ============================================================================ # State m0 = ca.DMatrix([0., 0., 5., 4., 15., 0., ca.pi/2]) S0 = ca.diagcat([1., 1., 1., 1., 1., 1., 1e-2]) * 0.25 L0 = ca.DMatrix.eye(nx) * 1e-3 L0[-1,-1] = 1e-5 mu0 = np.array(m0).ravel() # to get samples from normal distribution b0 = belief() b0['m'] = m0 b0['S'] = ca.densify(S0) eb0 = ext_belief() eb0['m'] = m0 eb0['S'] = ca.densify(S0) eb0['L'] = ca.densify(L0) # Covariances
F = ca.SXFunction('Discrete dynamics', [state, control, dt_sym], [state_next], op) Fj_x = F.jacobian('state') Fj_u = F.jacobian('control') F_xx = ca.SXFunction( 'F_xx', [state, control, dt_sym], [ca.jacobian(F.jac('state')[i, :].T, state) for i in range(nx)]) F_uu = ca.SXFunction( 'F_uu', [state, control, dt_sym], [ca.jacobian(F.jac('control')[i, :].T, control) for i in range(nx)]) F_ux = ca.SXFunction( 'F_ux', [state, control, dt_sym], [ca.jacobian(F.jac('control')[i, :].T, state) for i in range(nx)]) # Cost functions Qf = ca.diagcat([1., 1., 0.]) final_cost = 0.5 * ca.mul([state.cat.T, Qf, state.cat]) op = {'input_scheme': ['state'], 'output_scheme': ['cost']} lf = ca.SXFunction('Final cost', [state], [final_cost], op) R = ca.DMatrix.eye(nu) * dt * 1e-2 cost = 0.5 * ca.mul([control.cat.T, R, control.cat]) op = {'input_scheme': ['state', 'control', 'dt'], 'output_scheme': ['cost']} l = ca.SXFunction('Running cost', [state, control, dt_sym], [cost], op) # Quadratic expansions of costs lfg = lf.gradient('state') # lf_x lfh = lf.hessian('state') # lf_xx lg_x = l.gradient('state') # l_x lh_x = l.hessian('state') # l_xx
def __init__(self, dae, t, order, method='legendre', parallelization='serial', tdp_fun=None, expand=True, repeat_param=False, options={}): """Constructor @param t time vector of length N+1 defining N collocation intervals @param order number of collocation points per interval @param method collocation method ('legendre', 'radau') @param dae DAE model @param parallelization parallelization of the outer map. Possible set of values is the same as for casadi.Function.map(). @return Returns a dictionary with the following keys: 'X' -- state at collocation points 'Z' -- alg. state at collocation points 'x0' -- initial state 'eq' -- the expression eq == 0 defines the collocation equation. eq depends on X, Z, x0, p. 'Q' -- quadrature values at collocation points depending on x0, X, Z, p. """ # Convert whatever DAE to implicit DAE dae = dae.makeImplicit() M = order N = len(t) - 1 # # Define variables and functions corresponfing to all control intervals # K = cs.MX.sym('K', dae.nx, N * M) # State derivatives at collocation points Z = cs.MX.sym('Z', dae.nz, N * M) # Alg state at collocation points x = cs.MX.sym('x', dae.nx, N + 1) # State at the ends of collocation intervals (t) u = cs.MX.sym('u', dae.nu, N) # Input on collocation intervals U = cs.horzcat(*[cs.repmat(u[:, n], 1, M) for n in range(N)]) # Input at collocation points # Butcher tableau for the selected method butcher = butcherTableuForCollocationMethod(order, method) # Interval lengths h = np.diff(t) # Integrated state at collocation points Mx = cs.kron(cs.DM.eye(N), cs.DM.ones(1, M)) MK = cs.kron(cs.diagcat(*h), butcher.A.T) # integration matrix X = cs.mtimes(x[:, : -1], Mx) + cs.mtimes(K, MK) # Integrated state at the ends of collocation intervals xf = x[:, : -1] + cs.mtimes(K, cs.kron(cs.diagcat(*h), butcher.b)) # Points in time at which the collocation equations are calculated # TODO: this possibly can be sped up a little bit. tc = np.hstack([t[n] + h[n] * butcher.c for n in range(N)]) # Values of the time-dependent parameter if tdp_fun is not None: tdp_val = cs.horzcat(*[tdp_fun(t) for t in tc]) else: assert dae.ntdp == 0 tdp_val = np.zeros((0, tc.size)) # DAE function dae_fun = dae.createFunction('dae', ['xdot', 'x', 'z', 'u', 'p', 't', 'tdp'], ['dae', 'quad']) if expand: dae_fun = dae_fun.expand() # expand() for speed if repeat_param: reduce_in = [] p = cs.MX.sym('P', dae.np, N * M) else: reduce_in = [4] p = cs.MX.sym('P', dae.np) dae_map = dae_fun.map('dae_map', parallelization, N * M, reduce_in, [], options) dae_out = dae_map(xdot=K, x=X, z=Z, u=U, p=p, t=tc, tdp=tdp_val) eqc = ce.struct_MX([ ce.entry('collocation', expr=dae_out['dae']), ce.entry('continuity', expr=xf - x[:, 1 :]), ce.entry('param', expr=cs.diff(p, 1, 1)) ]) # Integrate the quadrature state quad = dae_out['quad'] #t0 = time.time() q = [cs.MX.zeros(dae.nq)] # Integrated quadrature at interval ends # TODO: speed up the calculation of q. for n in range(N): q.append(q[-1] + h[n] * cs.mtimes(quad[:, n * M : (n + 1) * M], butcher.b)) q = cs.horzcat(*q) Q = cs.mtimes(q[:, : -1], Mx) + cs.mtimes(quad, MK) # Integrated quadrature at collocation points #print('Creating Q took {0:.3f} s.'.format(time.time() - t0)) self._N = N self._M = M self._eq = eqc self._x = x self._X = X self._K = K self._Z = Z self._U = U self._u = u self._quad = quad self._Q = Q self._q = q self._p = p self._tc = tc self._butcher = butcher self._tdp = tdp_val self._t = t