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
