def onestep_reachability(p_center, ssm, k_ff, l_mu, l_sigma, q_shape=None, k_fb=None, c_safety=1., a=None, b=None, t_z_gp=None): """ Overapproximate the reachable set of states under affine control law given a system of the form: x_{t+1} = \mathcal{N}(\mu(x_t,u_t), \Sigma(x_t,u_t)), where x,\mu \in R^{n_s}, u \in R^{n_u} and \Sigma^{n_s \times n_s} are given bei the gp predictive mean and variance respectively we approximate the reachset of a set of inputs x_t \in \epsilon(p,Q) describing an ellipsoid with center p and shape matrix Q under the control low u_t = Kx_t + k Parameters ---------- p_center: n_s x 1 array[float] Center of state ellipsoid gp: SimpleGPModel The gp representing the dynamics k_ff: n_u x 1 array[float] The additive term of the controls l_mu: 1d_array of size n_s Set of Lipschitz constants on the Gradients of the mean function (per state dimension) l_sigma: 1d_array of size n_s Set of Lipschitz constants of the predictive variance (per state dimension) q_shape: np.ndarray[float], array of shape n_s x n_s, optional Shape matrix of state ellipsoid k_fb: n_u x n_s array[float], optional The state feedback-matrix for the controls c_safety: float, optional The scaling of the semi-axes of the uncertainty matrix corresponding to a level-set of the gaussian pdf. Returns: ------- p_new: n_s x 1 array[float] Center of the overapproximated next state ellipsoid Q_new: np.ndarray[float], array of shape n_s x n_s Shape matrix of the overapproximated next state ellipsoid. """ n_s = np.shape(p_center)[0] n_u = np.shape(k_ff)[0] if t_z_gp is None: t_z_gp = MX.eye(n_s) if a is None: a = MX.eye(n_s) b = MX.zeros(n_s, n_u) if q_shape is None: # the state is a point u_p = k_ff x_bar = mtimes(t_z_gp, p_center) mu_new, pred_var, _ = ssm(x_bar.T, u_p.T) p_lin = mtimes(a, p_center) + mtimes(b, u_p) p_1 = p_lin + mu_new rkhs_bound = c_safety * sqrt(pred_var) q_1 = ellipsoid_from_rectangle(rkhs_bound) return p_1, q_1, pred_var else: # the state is a (ellipsoid) set # compute the linearization centers x_bar = mtimes(t_z_gp, p_center) # center of the state ellipsoid u_bar = k_ff # u_bar = K*(u_bar-u_bar) + k = k z_bar = vertcat(x_bar, u_bar) # compute the zero and first order matrices mu_0, sigm_0, jac_mu = ssm(x_bar.T, u_bar.T) n_x_in = np.shape(t_z_gp)[0] a_mu = jac_mu[:, :n_x_in] a_mu = mtimes(a_mu, t_z_gp) b_mu = jac_mu[:, n_x_in:] # reach set of the affine terms H = a + a_mu + mtimes(b_mu + b, k_fb) p_0 = mu_0 + mtimes(a, p_center) + mtimes(b, u_bar) Q_0 = mtimes(H, mtimes(q_shape, H.T)) ub_mean, ub_sigma = compute_remainder_overapproximations( q_shape, k_fb, l_mu, l_sigma) # computing the box approximate to the lagrange remainder Q_lagrange_mu = ellipsoid_from_rectangle(ub_mean) p_lagrange_mu = MX.zeros((n_s, 1)) b_sigma_eps = c_safety * (sqrt(sigm_0) + ub_sigma) Q_lagrange_sigm = ellipsoid_from_rectangle(b_sigma_eps) p_lagrange_sigm = MX.zeros((n_s, 1)) p_sum_lagrange, Q_sum_lagrange = sum_two_ellipsoids( p_lagrange_sigm, Q_lagrange_sigm, p_lagrange_mu, Q_lagrange_mu) p_1, q_1 = sum_two_ellipsoids(p_sum_lagrange, Q_sum_lagrange, p_0, Q_0) return p_1, q_1, sigm_0
def __init__(self, T, gp, env_options, beta_safety, lin_trafo_gp_input=None, perf_trajectory="mean_equivalent", k_fb=None): self.T = T self.gp = gp self.n_s = gp.n_s_out self.n_u = gp.n_u self.n_fail = T - 1 self.beta_safety = beta_safety self._set_perf_trajectory(perf_trajectory) self.opt_x0 = False self.cost_func = None self.lin_prior = False self._set_attributes_from_dict(ATTR_NAMES_ENV, DEFAULT_OPT_ENV, env_options) self.lin_trafo_gp_input = lin_trafo_gp_input if self.lin_trafo_gp_input is None: self.lin_trafo_gp_input = MX.eye(self.n_s) if self.h_mat_obs is None: m_obs_mat = 0 else: m_obs_mat, n_s_obs = np.shape(self.h_mat_obs) assert n_s_obs == self.n_s, " Wrong shape of obstacle matrix" assert np.shape(self.h_obs) == (m_obs_mat, 1), \ " Shapes of obstacle linear inequality matrix/vector must match " self.m_obs = m_obs_mat self.has_ctrl_bounds = False if self.ctrl_bounds is not None: self.has_ctrl_bounds = True assert np.shape(self.ctrl_bounds) == (self.n_u, 2), """control bounds need to be of shape n_u x 2 with i,0 lower bound and i,1 upper bound per dimension""" m_safe_mat, n_s_safe = np.shape(self.h_mat_safe) assert n_s_safe == self.n_s, " Wrong shape of safety matrix" assert np.shape(self.h_safe) == (m_safe_mat, 1), \ " Shapes of safety linear inequality matrix/vector must match." self.m_safe = m_safe_mat self.a = np.eye(self.n_s) self.b = np.zeros((self.n_s, self.n_u)) if self.lin_model is not None: self.a, self.b = self.lin_model self.eval_prior = lambda x, u: np.dot(x, self.a.T) + np.dot( u, self.b.T) self.k_fb = k_fb