def plot_partition(self, constraint, dims, color):
# This if shouldn't really be necessary -- someone is calling self.plot_partitions with something other than a (constraint, ___) element in M?
if isinstance(constraint, np.ndarray):
constraint = constraints.LpConstraint(range=constraint)
constraint.plot(self.animate_axes, dims, color, linewidth=1, plot_2d=self.plot_2d)
def plot_partitions(self, M, output_constraint, dims):
# first = True
for (input_constraint, output_range) in M:
# if first:
# input_label = "Cell of Partition"
# output_label = "One Cell's Estimated Bounds"
# first = False
# else:
# input_label = None
# output_label = None
# Next state constraint of that cell
output_constraint_ = constraints.LpConstraint(range=output_range)
self.plot_partition(output_constraint_, dims, "grey")
# Initial state constraint of that cell
self.plot_partition(input_constraint, dims, "tab:red")
def call_visualizer(self,
output_range_sim,
M,
num_propagator_calls,
interior_M,
iteration,
dont_tighten_layout=False):
u_e = self.squash_down_to_one_range(output_range_sim, M)
# title = "# Partitions: {}, Error: {}".format(str(len(M)+len(interior_M)), str(round(error, 3)))
title = "# Propagator Calls: {}".format(str(int(num_propagator_calls)))
# title = None
output_constraint = constraints.LpConstraint(range=u_e)
self.visualize(M,
interior_M,
output_constraint,
iteration=iteration,
title=title,
reachable_set_color=self.reachable_set_color,
reachable_set_zorder=self.reachable_set_zorder,
dont_tighten_layout=dont_tighten_layout)
def get_reachable_set(
self,
input_constraint,
output_constraint,
propagator,
t_max,
num_partitions=None,
):
t_start_overall = time.time()
info = {}
propagator_computation_time = 0
# Algorithm 1 of (Xiang, 2020): https://arxiv.org/pdf/2004.12273.pdf
sect_method = "max"
num_propagator_calls = 0
interior_M = []
# Run N simulations (i.e., randomly sample N pts from input range -->
# query NN --> get N output pts)
# Compute [u_sim], aka bounds on the sampled outputs (Line 6)
# (Line 5-6)
output_range_sim = self.get_sampled_out_range_guidance(
input_constraint, propagator, t_max, num_samples=1000)
# Get initial output reachable set (Line 3)
t_start = time.time()
output_constraint_, info = propagator.get_reachable_set(
input_constraint, deepcopy(output_constraint), t_max)
t_end = time.time()
propagator_computation_time += t_end - t_start
num_propagator_calls += t_max
if isinstance(output_constraint, constraints.PolytopeConstraint):
raise NotImplementedError
elif isinstance(output_constraint, constraints.LpConstraint):
reachable_set = [o.range for o in output_constraint_]
M = [(input_constraint, reachable_set)] # (Line 4)
else:
raise NotImplementedError
u_e = reachable_set.copy()
if self.make_animation:
output_constraint_ = constraints.LpConstraint(
range=[o.range for o in output_constraint_])
self.setup_visualization(
input_constraint,
output_constraint_,
propagator,
show_samples=True,
inputs_to_highlight=[
{
"dim": [0],
"name": "$x_0$"
},
{
"dim": [1],
"name": "$x_1$"
},
],
aspect="auto",
initial_set_color=self.initial_set_color,
initial_set_zorder=self.initial_set_zorder,
sample_zorder=self.sample_zorder)
u_e, info = self.partition_loop(
M,
interior_M,
output_range_sim,
sect_method,
num_propagator_calls,
input_constraint,
u_e,
propagator,
propagator_computation_time,
t_start_overall,
t_max,
output_constraint,
)
# info["all_partitions"] = ranges
info["num_propagator_calls"] = num_propagator_calls
info["num_partitions"] = np.product(num_partitions)
output_constraint.range = u_e
return output_constraint, info
def partition_loop(self, M, interior_M, output_range_sim, sect_method,
num_propagator_calls, input_constraint, u_e, propagator,
propagator_computation_time, t_start_overall, t_max,
output_constraint):
if self.make_animation:
self.call_visualizer(output_range_sim,
M + interior_M,
num_propagator_calls,
interior_M,
iteration=-1)
# Used by UnGuided, SimGuided, GreedySimGuided, etc.
iteration = 0
terminate = False
start_time_partition_loop = t_start_overall
while len(M) != 0 and not terminate:
input_constraint_, reachable_set_ = self.grab_from_M(
M, output_range_sim) # (Line 9)
if self.check_if_partition_within_sim_bnds(reachable_set_,
output_range_sim):
# Line 11
interior_M.append((input_constraint_, reachable_set_))
else:
# Line 14
elapsed_time = time.time() - start_time_partition_loop
terminate = self.check_termination(
input_constraint,
num_propagator_calls,
u_e,
output_range_sim,
M + [(input_constraint_, reachable_set_)] + interior_M,
elapsed_time,
)
if not terminate:
# Line 15
input_ranges_ = sect(input_constraint_.range,
2,
select=sect_method)
# Lines 16-17
for input_range_ in input_ranges_:
t_start = time.time()
input_constraint_ = constraints.LpConstraint(
range=input_range_)
output_constraint_, info = propagator.get_reachable_set(
input_constraint_, deepcopy(output_constraint),
t_max)
t_end = time.time()
propagator_computation_time += t_end - t_start
num_propagator_calls += t_max
reachable_set_ = [o.range for o in output_constraint_]
M.append(
(input_constraint_, reachable_set_)) # Line 18
else: # Lines 19-20
M.append((input_constraint_, reachable_set_))
if self.make_animation:
self.call_visualizer(output_range_sim,
M + interior_M,
num_propagator_calls,
interior_M,
iteration=iteration,
dont_tighten_layout=False)
iteration += 1
# Line 24
u_e = self.squash_down_to_one_range(output_range_sim, M + interior_M)
# u_e = self.squash_down_to_one_range(output_range_sim, M)
t_end_overall = time.time()
ranges = []
for m in M + interior_M:
ranges.append((m[0].range, np.stack(m[1])))
info["all_partitions"] = ranges
# Stats & Visualization
# info = self.compile_info(
# output_range_sim,
# M,
# interior_M,
# num_propagator_calls,
# t_end_overall,
# t_start_overall,
# propagator_computation_time,
# iteration,
# )
if self.make_animation:
self.compile_animation(iteration,
delete_files=False,
start_iteration=-1)
return u_e, info
def main(args):
np.random.seed(seed=0)
stats = {}
# Load NN control policy
controller = load_controller(name=args.system)
# Dynamics
if args.system == "double_integrator":
inputs_to_highlight = [
{"dim": [0], "name": "$x_0$"},
{"dim": [1], "name": "$x_1$"},
]
if args.state_feedback:
dyn = dynamics.DoubleIntegrator()
else:
dyn = dynamics.DoubleIntegratorOutputFeedback()
if args.init_state_range is None:
init_state_range = np.array(
[ # (num_inputs, 2)
[2.5, 3.0], # x0min, x0max
[-0.25, 0.25], # x1min, x1max
]
)
else:
import ast
init_state_range = np.array(
ast.literal_eval(args.init_state_range)
)
elif args.system == "quadrotor":
inputs_to_highlight = [
{"dim": [0], "name": "$x$"},
{"dim": [1], "name": "$y$"},
{"dim": [2], "name": "$z$"},
]
if args.state_feedback:
dyn = dynamics.Quadrotor()
else:
dyn = dynamics.QuadrotorOutputFeedback()
if args.init_state_range is None:
init_state_range = np.array(
[ # (num_inputs, 2)
[4.65, 4.65, 2.95, 0.94, -0.01, -0.01],
[4.75, 4.75, 3.05, 0.96, 0.01, 0.01],
]
).T
else:
import ast
init_state_range = np.array(
ast.literal_eval(args.init_state_range)
)
elif args.system == "duffing":
inputs_to_highlight = [
{"dim": [0], "name": "$x_0$"},
{"dim": [1], "name": "$x_1$"},
]
dyn = dynamics.Duffing()
init_state_range = np.array(
[ # (num_inputs, 2)
[2.45, 2.55], # x0min, x0max
[1.45, 1.55], # x1min, x1max
]
)
elif args.system == "iss":
inputs_to_highlight = [
{"dim": [0], "name": "$x_0$"},
{"dim": [1], "name": "$x_1$"},
]
dyn = dynamics.ISS()
init_state_range = 100 * np.ones((dyn.n, 2))
init_state_range[:, 0] = init_state_range[:, 0] - 0.5
init_state_range[:, 1] = init_state_range[:, 1] + 0.5
elif args.system == "unity":
inputs_to_highlight = [
{"dim": [0], "name": "$x$"},
{"dim": [1], "name": "$y$"},
]
dyn = dynamics.Unity(args.nx, args.nu)
if args.init_state_range is None:
init_state_range = np.vstack([-np.ones(args.nx), np.ones(args.nx)]).T
else:
import ast
init_state_range = np.array(
ast.literal_eval(args.init_state_range)
)
controller = load_controller_unity(args.nx, args.nu)
else:
raise NotImplementedError
if args.num_partitions is None:
num_partitions = np.array([4, 4])
else:
import ast
num_partitions = np.array(
ast.literal_eval(args.num_partitions)
)
partitioner_hyperparams = {
"type": args.partitioner,
"num_partitions": num_partitions,
"make_animation": args.make_animation,
"show_animation": args.show_animation,
}
propagator_hyperparams = {
"type": args.propagator,
"input_shape": init_state_range.shape[:-1],
}
if args.propagator == "SDP":
propagator_hyperparams["cvxpy_solver"] = args.cvxpy_solver
# Set up analyzer (+ parititoner + propagator)
analyzer = analyzers.ClosedLoopAnalyzer(controller, dyn)
analyzer.partitioner = partitioner_hyperparams
analyzer.propagator = propagator_hyperparams
# Set up initial state set (and placeholder for reachable sets)
if args.boundaries == "polytope":
A_inputs, b_inputs = range_to_polytope(init_state_range)
if args.system == "quadrotor":
A_out = A_inputs
else:
A_out = get_polytope_A(args.num_polytope_facets)
input_constraint = constraints.PolytopeConstraint(
A_inputs, b_inputs
)
output_constraint = constraints.PolytopeConstraint(A_out)
elif args.boundaries == "lp":
input_constraint = constraints.LpConstraint(
range=init_state_range, p=np.inf
)
output_constraint = constraints.LpConstraint(p=np.inf)
else:
raise NotImplementedError
if args.estimate_runtime:
# Run the analyzer N times to compute an estimated runtime
import time
num_calls = 5
times = np.empty(num_calls)
final_errors = np.empty(num_calls)
avg_errors = np.empty(num_calls, dtype=np.ndarray)
all_errors = np.empty(num_calls, dtype=np.ndarray)
output_constraints = np.empty(num_calls, dtype=object)
for num in range(num_calls):
print('call: {}'.format(num))
t_start = time.time()
output_constraint, analyzer_info = analyzer.get_reachable_set(
input_constraint, output_constraint, t_max=args.t_max
)
t_end = time.time()
t = t_end - t_start
times[num] = t
final_error, avg_error, all_error = analyzer.get_error(input_constraint, output_constraint, t_max=args.t_max)
final_errors[num] = final_error
avg_errors[num] = avg_error
all_errors[num] = all_error
output_constraints[num] = output_constraint
stats['runtimes'] = times
stats['final_step_errors'] = final_errors
stats['avg_errors'] = avg_errors
stats['all_errors'] = all_errors
stats['output_constraints'] = output_constraints
print("All times: {}".format(times))
print("Avg time: {} +/- {}".format(times.mean(), times.std()))
else:
# Run analysis once
output_constraint, analyzer_info = analyzer.get_reachable_set(
input_constraint, output_constraint, t_max=args.t_max
)
if args.estimate_error:
final_error, avg_error, errors = analyzer.get_error(input_constraint, output_constraint, t_max=args.t_max)
print('Final step approximation error:{:.2f}\nAverage approximation error: {:.2f}\nAll errors: {}'.format(final_error, avg_error, errors))
if args.save_plot:
save_dir = "{}/results/examples/".format(
os.path.dirname(os.path.abspath(__file__))
)
os.makedirs(save_dir, exist_ok=True)
# Ugly logic to embed parameters in filename:
pars = "_".join(
[
str(key) + "_" + str(value)
for key, value in sorted(
partitioner_hyperparams.items(), key=lambda kv: kv[0]
)
if key
not in [
"make_animation",
"show_animation",
"type",
"num_partitions",
]
]
)
pars2 = "_".join(
[
str(key) + "_" + str(value)
for key, value in sorted(
propagator_hyperparams.items(), key=lambda kv: kv[0]
)
if key not in ["input_shape", "type"]
]
)
analyzer_info["save_name"] = (
save_dir
+ args.system
+ pars
+ "_"
+ partitioner_hyperparams["type"]
+ "_"
+ propagator_hyperparams["type"]
+ "_"
+ "tmax"
+ "_"
+ str(round(args.t_max, 1))
+ "_"
+ args.boundaries
+ "_"
+ str(args.num_polytope_facets)
)
if len(pars2) > 0:
analyzer_info["save_name"] = (
analyzer_info["save_name"] + "_" + pars2
)
analyzer_info["save_name"] = analyzer_info["save_name"] + ".png"
if args.show_plot or args.save_plot:
analyzer.visualize(
input_constraint,
output_constraint,
show_samples=True,
show=args.show_plot,
labels=args.plot_labels,
aspect=args.plot_aspect,
iteration=None,
inputs_to_highlight=inputs_to_highlight,
**analyzer_info
)
return stats, analyzer_info
def main(args):
np.random.seed(seed=0)
stats = {}
# Load NN control policy
controller = load_controller(name=args.system)
# Dynamics
if args.system == "double_integrator":
if args.state_feedback:
dyn = dynamics.DoubleIntegrator()
else:
raise NotImplementedError
dyn = dynamics.DoubleIntegratorOutputFeedback()
if args.final_state_range is None:
final_state_range = np.array(
[ # (num_inputs, 2)
[2.5, 3.0], # x0min, x0max
[-0.25, 0.25], # x1min, x1max
]
)
else:
raise NotImplementedError
import ast
init_state_range = np.array(
ast.literal_eval(args.init_state_range)
)
else:
raise NotImplementedError
if args.num_partitions is None:
num_partitions = np.array([2, 2])
else:
import ast
num_partitions = np.array(
ast.literal_eval(args.num_partitions)
)
partitioner_hyperparams = {
"type": args.partitioner,
"num_partitions": num_partitions,
}
propagator_hyperparams = {
"type": args.propagator,
"input_shape": final_state_range.shape[:-1],
}
# Set up analyzer (+ partitioner + propagator)
analyzer = analyzers.ClosedLoopBackwardAnalyzer(controller, dyn)
analyzer.partitioner = partitioner_hyperparams
analyzer.propagator = propagator_hyperparams
# Set up initial state set (and placeholder for reachable sets)
if args.boundaries == "polytope":
A_out, b_out = range_to_polytope(final_state_range)
output_constraint = constraints.PolytopeConstraint(
A=A_out, b=[b_out]
)
input_constraint = constraints.PolytopeConstraint(None, None)
elif args.boundaries == "lp":
output_constraint = constraints.LpConstraint(
range=final_state_range, p=np.inf
)
input_constraint = constraints.LpConstraint(p=np.inf, range=None)
else:
raise NotImplementedError
if args.estimate_runtime:
# Run the analyzer N times to compute an estimated runtime
import time
num_calls = 5
times = np.empty(num_calls)
final_errors = np.empty(num_calls)
avg_errors = np.empty(num_calls, dtype=np.ndarray)
all_errors = np.empty(num_calls, dtype=np.ndarray)
output_constraints = np.empty(num_calls, dtype=object)
for num in range(num_calls):
print('call: {}'.format(num))
t_start = time.time()
input_constraint, analyzer_info = analyzer.get_backprojection_set(
output_constraint, input_constraint, t_max=None, num_partitions=num_partitions
)
t_end = time.time()
t = t_end - t_start
times[num] = t
stats['runtimes'] = times
print("All times: {}".format(times))
print("Avg time: {} +/- {}".format(times.mean(), times.std()))
else:
# Run analysis once
# Run analysis & generate a plot
input_constraint, analyzer_info = analyzer.get_backprojection_set(
output_constraint, input_constraint, t_max=None, num_partitions=num_partitions
)
# print(input_constraint.A, input_constraint.b)
# error, avg_error = analyzer.get_error(input_constraint,output_constraint, t_max=args.t_max)
# print('Final step approximation error:{:.2f}\nAverage approximation error: {:.2f}'.format(error, avg_error))
if args.save_plot:
save_dir = "{}/results/examples_backward/".format(
os.path.dirname(os.path.abspath(__file__))
)
os.makedirs(save_dir, exist_ok=True)
# Ugly logic to embed parameters in filename:
pars = "_".join(
[
str(key) + "_" + str(value)
for key, value in sorted(
partitioner_hyperparams.items(), key=lambda kv: kv[0]
)
if key
not in [
"make_animation",
"show_animation",
"type",
"num_partitions",
]
]
)
pars2 = "_".join(
[
str(key) + "_" + str(value)
for key, value in sorted(
propagator_hyperparams.items(), key=lambda kv: kv[0]
)
if key not in ["input_shape", "type"]
]
)
analyzer_info["save_name"] = (
save_dir
+ args.system
+ pars
+ "_"
+ partitioner_hyperparams["type"]
+ "_"
+ propagator_hyperparams["type"]
+ "_"
# + "tmax"
# + "_"
# + str(round(args.t_max, 1))
# + "_"
+ args.boundaries
+ "_"
+ str(args.num_polytope_facets)
+ "_"
+ "partitions"
+ "_"
+ np.array2string(num_partitions, separator='_')[1:-1]
)
if len(pars2) > 0:
analyzer_info["save_name"] = (
analyzer_info["save_name"] + "_" + pars2
)
analyzer_info["save_name"] = analyzer_info["save_name"] + ".png"
if args.show_plot or args.save_plot:
analyzer.visualize(
input_constraint[0],
output_constraint,
show_samples=True,
show=args.show_plot,
labels=args.plot_labels,
aspect=args.plot_aspect,
plot_lims=args.plot_lims,
**analyzer_info
)
return stats
return xs_t1
if __name__ == "__main__":
from nn_closed_loop.dynamics.DoubleIntegrator import DoubleIntegrator
dynamics = DoubleIntegrator()
init_state_range = np.array([
# (num_inputs, 2)
[2.5, 3.0], # x0min, x0max
[-0.25, 0.25], # x1min, x1max
])
xs, us = dynamics.collect_data(
t_max=10,
input_constraint=constraints.LpConstraint(
p=np.inf, range=init_state_range
),
num_samples=2420,
)
print(xs.shape, us.shape)
system = "double_integrator"
with open(dir_path + "/../../datasets/{}/xs.pkl".format(system), "wb") as f:
pickle.dump(xs, f)
with open(dir_path + "/../../datasets/{}/us.pkl".format(system), "wb") as f:
pickle.dump(us, f)
# from nn_closed_loop.utils.nn import load_model
# # dynamics = DoubleIntegrator()
# # init_state_range = np.array([ # (num_inputs, 2)
# # [2.5, 3.0], # x0min, x0max