def initialize_scenario(self,
robot_id: int,
prediction_horizon: int,
system_matrices: dict,
cost_matrices: dict,
coupling_constraints: dict,
local_constraints: dict = None):
# save information
self.prediction_horizon = prediction_horizon
self.system_matrices = system_matrices
self.coupling_constraints = coupling_constraints
self.robot_id = robot_id
# shorhands
A = system_matrices['A']
B = system_matrices['B']
T = prediction_horizon
E = local_constraints['x_matrix']
f = local_constraints['x_vector']
G = local_constraints['u_matrix']
h = local_constraints['u_vector']
Q = cost_matrices['state']
R = cost_matrices['input']
n, m = self._sizes = (A.shape[0], B.shape[1])
# create optimization variables
z = Variable((T + 1) * n + T * m) # complete optimization variable
self._x = np.vstack((np.eye((T + 1) * n), np.zeros(
(T * m,
(T + 1) * n)))) @ z # state portion of optimization variable
self._u = np.vstack((np.zeros(((T + 1) * n, T * m)), np.eye(
T * m))) @ z # input portion of optimization variable
# initialize objective function and constraints
self._obj_func = 0
self._basic_constraints = []
for t in range(T):
# extract optimization variables corresponding to x(t), u(t), x(t+1)
x_t = np.eye(n, (T + 1) * n, t * n).T @ self._x
u_t = np.eye(m, T * m, t * m).T @ self._u
x_tp = np.eye(n, (T + 1) * n, (t + 1) * n).T @ self._x
# build local constraints
self._basic_constraints.append(x_tp == A.T @ x_t +
B.T @ u_t) # dynamics
self._basic_constraints.append(E.T @ x_t <= f) # state
self._basic_constraints.append(G.T @ u_t <= h) # input
# add terms to objective function
self._obj_func += QuadraticForm(x_t, Q) + QuadraticForm(u_t, R)
from disropt.functions import QuadraticForm, Variable
from disropt.utils.graph_constructor import MPIgraph
from disropt.problems import Problem
# generate communication graph (everyone uses the same seed)
comm_graph = MPIgraph('random_binomial', 'metropolis')
agent_id, in_nbrs, out_nbrs, in_weights, _ = comm_graph.get_local_info()
# size of optimization variable
n = 5
# generate quadratic cost function
np.random.seed()
Q = np.random.randn(n, n)
Q = Q.transpose() @ Q
x = Variable(n)
func = QuadraticForm(x - np.random.randn(n, 1), Q)
# create Problem and Agent
agent = Agent(in_nbrs, out_nbrs, in_weights=in_weights)
agent.set_problem(Problem(func))
# run the algorithm
x0 = np.random.rand(n, 1)
algorithm = GradientTracking(agent, x0)
algorithm.run(iterations=1000, stepsize=0.01)
print("Agent {} - solution estimate: {}".format(
agent_id,
algorithm.get_result().flatten()))
agent = Agent(in_neighbors=np.nonzero(Adj[local_rank, :])[0].tolist(),
out_neighbors=np.nonzero(Adj[:, local_rank])[0].tolist(),
in_weights=W[local_rank, :].tolist())
# variable dimension
n = 6
# generate a positive definite matrix
P = np.random.randn(n, n)
P = P.transpose() @ P
bias = np.random.randn(n, 1)
# declare a variable
x = Variable(n)
# define the local objective function
fn = QuadraticForm(x - bias, P)
# define a (common) constraint set
constr = [x <= 1, x >= -1]
# local problem
pb = Problem(fn, constr)
agent.set_problem(pb)
# instantiate the algorithms
initial_condition = np.random.rand(n, 1)
algorithm = BlockSubgradientMethod(agent=agent,
initial_condition=initial_condition,
enable_log=True)
np.random.seed(10*agent_id)
# linear objective function
dim = 2
z = Variable(dim)
c = np.ones([dim,1])
obj_func = c @ z
# constraints are circles of the form (z-p)^\top (z-p) <= 1
# equivalently z^\top z - 2(A p)^\top z + p^\top A p <= 1
I = np.eye(dim)
p = np.random.rand(dim,1)
r = 1 # unitary radius
constr = []
ff = QuadraticForm(z,I,- 2*(I @ p),(p.transpose() @ I @ p) - r**2)
constr.append(ff<= 0)
#####################
# Distributed algorithms
#####################
# local agent and problem
agent = Agent(
in_neighbors=np.nonzero(Adj[agent_id, :])[0].tolist(),
out_neighbors=np.nonzero(Adj[:, agent_id])[0].tolist())
pb = Problem(obj_func, constr)
agent.set_problem(pb)
# instantiate the algorithm
algorithm = ConstraintsConsensus(agent=agent,