def generate_variable_limits(self, hidden_size, num_inputs, action_space,
bounds):
var_lb, var_ub = bounds
layer1_weights = [[0 for i in range(num_inputs)]
for j in range(hidden_size)] #placeholder
for i in range(hidden_size):
for j in range(num_inputs):
layer1_weights[i][j] = dreal.Variable("A" + str(i) + "_" +
str(j))
layer2_weights = [[0 for i in range(hidden_size)]
for j in range(action_space.n)]
for i in range(action_space.n):
for j in range(hidden_size):
layer2_weights[i][j] = dreal.Variable("B" + str(i) + "_" +
str(j))
limits = [] #does this change performance?
for l in layer1_weights:
for p in l:
limits.append((p <= var_ub))
limits.append((p >= var_lb))
for l in layer2_weights:
for p in l:
limits.append((p <= var_ub))
limits.append((p >= var_lb))
return (layer1_weights, layer2_weights), limits
def __init__(self, bounds, variables=None):
self.bounds = bounds
self.widths = [_[1] - _[0] for _ in bounds]
if variables:
self.variables = variables
else:
self.variables = [
dr.Variable('var' + str(i)) for i in range(len(bounds))
]
def generate_variable_limits(self, hidden_size, num_inputs, action_space,
bounds):
lb, ub = bounds
coef_vars = []
limits = []
for i in range(hidden_size):
v = dreal.Variable(str(i))
coef_vars.append(v)
limits.append((v <= ub))
limits.append((v >= lb))
return coef_vars, limits
def __init__(self, n_vars, f, learner_type, verifier_type, inner_radius, outer_radius, \
equilibria, n_hidden_neurons, activations, linear_factor=False):
self.n = n_vars
self.f = f
self.learner_type = learner_type
self.inner = inner_radius
self.outer = outer_radius
self.h = n_hidden_neurons
self.max_cegis_iter = 5
# batch init
self.batch_size = 500
self.learning_rate = .1
if verifier_type == VerifierType.Z3:
self.x = [Real('x%d' % i) for i in range(n_vars)]
else:
self.x = [dr.Variable('x%d' % i) for i in range(n_vars)]
self.x_map = {str(x): x for x in self.x}
# issues w/ dimensionality, maybe could be solved better
if self.n > 1:
self.xdot = f(self.x)
else:
self.xdot = f(self.x[0])
self.x = np.matrix(self.x).T
self.xdot = np.matrix(self.xdot).T
self.eq = equilibria
if learner_type == LearnerType.NN:
self.learner = NN(n_vars,
*n_hidden_neurons,
bias=True,
activate=activations,
equilibria=self.eq)
elif learner_type == LearnerType.Z3:
self.learner = SimpleZ3Learner(self.n)
elif learner_type == LearnerType.SCIPY:
self.learner = ScipyLearner(self.n)
else:
print('M8 I aint got this learner')
if verifier_type == VerifierType.Z3:
verifier = Z3Verifier
elif verifier_type == VerifierType.DREAL:
verifier = DRealVerifier
else:
raise ValueError('No verifier of type {}'.format(verifier_type))
self.verifier = verifier(self.n, self.eq, self.inner, self.outer,
self.x)
# factorisation option
self.lf = linear_factor
def calc_interrobot_cost(trajectory, ob, config, xs, x_i, reach_goal):
min_t = config.predict_time
my_V = float(trajectory[-1, 3])
my_Y = float(trajectory[-1, 4])
# collision checking with dreal
for i in range(len(xs)):
if i != x_i and reach_goal[i] != 1:
oth_V = float(xs[i][3])
oth_Y = float(xs[i][4])
t = dreal.Variable("t")
my_x = dreal.Variable("my_x")
my_y = dreal.Variable("my_y")
oth_x = dreal.Variable("oth_x")
oth_y = dreal.Variable("oth_y")
distance_sq = dreal.Variable("distance_sq")
f_sat = dreal.And(
0 <= t, t <= config.predict_time,
my_x == xt(t, float(xs[x_i][0]), float(xs[x_i][2]), my_V,
my_Y), my_y == yt(t, float(xs[x_i][1]),
float(xs[x_i][2]), my_V, my_Y),
oth_x == xt(t, float(xs[i][0]), float(xs[i][2]), oth_V,
oth_Y), oth_y == yt(t, float(xs[i][1]),
float(xs[i][2]), oth_V, oth_Y),
distance_sq == (my_x - oth_x)**2 + (my_y - oth_y)**2,
distance_sq == (config.robot_radius * 2.1)**2)
result = dreal.CheckSatisfiability(f_sat, 0.1)
if result is not None:
min_t = min(result[t].lb(), min_t)
# print("probable collision", x_i, i, result, predict_time, min_d)
# return float("Inf")
if min_t == 0: return float("Inf")
return 1.0 / min_t - 1.0 / config.predict_time
'''
For learning
'''
N = 500 # sample size
D_in = 2 # input dimension
H1 = 15 # hidden dimension
D_out = 1 # output dimension
torch.manual_seed(10)
x = torch.Tensor(N, D_in).uniform_(-1.25, 1.25)
x_0 = torch.zeros([1, 2])
'''
For verifying
'''
x1 = dreal.Variable("x1")
x2 = dreal.Variable("x2")
vars_ = [x1, x2]
config = dreal.Config()
config.use_polytope_in_forall = True
config.use_local_optimization = True
config.precision = 1e-2
epsilon = 0
# Checking candidate V within a ball around the origin (ball_lb ≤ sqrt(∑xᵢ²) ≤ ball_ub)
ball_lb = 0.25
ball_ub = 1.25
# Learning and Falsification
def test_variable(self):
x1 = dreal.Variable('x')
x2 = odr_test_module.new_variable('x')
self.assertNotEqual(x1.get_id(), x2.get_id())
def formula(self, delta=0, strict=False):
"""Return a dReal formula expressing membership in this region."""
conjuncts = []
delta = abs(delta)
for bound, var in zip(self.bounds, self.variables):
if strict:
conjuncts.append(bound[0] + delta < var)
conjuncts.append(var < bound[1] - delta)
else:
conjuncts.append(bound[0] + delta <= var)
conjuncts.append(var <= bound[1] - delta)
return dr.And(*conjuncts)
DREAL_X, DREAL_Y, DREAL_T, DREAL_XINT = dr.Variable('x'), dr.Variable(
'y'), dr.Variable('t'), dr.Variable('xint')
TUNCALI_PHASE_X0 = AxisAlignedRange([[-1, 1], [-pi / 16, pi / 16]],
variables=[DREAL_X, DREAL_T])
TUNCALI_PHASE_U = AxisAlignedRange([[-5, 5], [-pi / 2, pi / 2]],
variables=[DREAL_X, DREAL_T])
TUNCALI_X0 = AxisAlignedRange([[-1, 1], [0, 0], [-pi / 16, pi / 16]],
variables=[DREAL_X, DREAL_Y, DREAL_T])
TUNCALI_U = AxisAlignedRange([[-5, 5], [0, 0], [-pi / 2, pi / 2]],
variables=[DREAL_X, DREAL_Y, DREAL_T])
RECURRENT_TRAIN = AxisAlignedRange([[-5, 5], [-pi / 2, pi / 2], [-10, 10]],
variables=[DREAL_X, DREAL_T, DREAL_XINT])
RECURRENT_TEST = AxisAlignedRange([[-2, 2], [-pi / 2, pi / 2], [0, 0]],
variables=[DREAL_X, DREAL_T, DREAL_XINT])
def calc_obstacle_cost(trajectory, ob, config, xs, x_i, reach_goal, traj_win):
"""
calc obstacle cost inf: collision
"""
ox = ob[:, 0]
oy = ob[:, 1]
dx = trajectory[:, 0] - ox[:, None]
dy = trajectory[:, 1] - oy[:, None]
r = np.hypot(dx, dy)
# calculate the distance between robots in order to determine cost
ox2 = np.array([])
oy2 = np.array([])
for i in range(len(xs)):
if i != x_i and reach_goal[i] != 1:
ox2 = np.append(ox2, xs[i][0])
oy2 = np.append(oy2, xs[i][1])
dx2 = trajectory[:, 0] - ox2[:, None]
dy2 = trajectory[:, 1] - oy2[:, None]
r2 = np.hypot(dx2, dy2)
# trajectory window for the current robot
min_dx = float(np.min(trajectory[:, 0]))
max_dx = float(np.max(trajectory[:, 0]))
min_dy = float(np.min(trajectory[:, 1]))
max_dy = float(np.max(trajectory[:, 1]))
# did not consider collision among rectangle robots yet
if config.robot_type == RobotType.rectangle:
yaw = trajectory[:, 2]
rot = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
rot = np.transpose(rot, [2, 0, 1])
local_ob = ob[:, None] - trajectory[:, 0:2]
local_ob = local_ob.reshape(-1, local_ob.shape[-1])
local_ob = np.array([local_ob @ x for x in rot])
local_ob = local_ob.reshape(-1, local_ob.shape[-1])
upper_check = local_ob[:, 0] <= config.robot_length / 2
right_check = local_ob[:, 1] <= config.robot_width / 2
bottom_check = local_ob[:, 0] >= -config.robot_length / 2
left_check = local_ob[:, 1] >= -config.robot_width / 2
if (np.logical_and(np.logical_and(upper_check, right_check),
np.logical_and(bottom_check, left_check))).any():
return float("Inf")
elif config.robot_type == RobotType.circle:
if (r <= config.robot_radius).any() or (r2 <=
config.robot_radius * 2).any():
return float("Inf")
# collision checking with dreal
for i in range(len(xs)):
if i != x_i and reach_goal[i] != 1:
my_x = dreal.Variable("my_x")
my_y = dreal.Variable("my_y")
oth_x = dreal.Variable("oth_x")
oth_y = dreal.Variable("oth_y")
f_sat = dreal.And(min_dx <= my_x, max_dx >= my_x,
min_dy <= my_y, max_dy >= my_y,
traj_win[i][0] <= oth_x,
traj_win[i][1] >= oth_x,
traj_win[i][2] <= oth_y,
traj_win[i][3] >= oth_y,
(my_x - oth_x)**2 + (my_y - oth_y)**2 <= 4)
result = dreal.CheckSatisfiability(f_sat, 0.001)
if result is not None:
return float("Inf")
if r2.size == 0: # this means only one robot remains and collision checking among robots is unneccessary
min_r = np.min([np.min(r) - config.robot_radius])
else:
min_r = np.min([
np.min(r) - config.robot_radius,
np.min(r2) - config.robot_radius * 2
])
return 1.0 / min_r