diff --git a/dpilqr/__init__.py b/dpilqr/__init__.py index eb6dea9..149cebb 100644 --- a/dpilqr/__init__.py +++ b/dpilqr/__init__.py @@ -8,10 +8,10 @@ quadraticize_distance, quadraticize_finite_difference, ) -from .decentralized import ( +from .distributed import ( define_inter_graph_threshold, solve_centralized, - solve_decentralized, + solve_distributed, solve_rhc, ) from .dynamics import ( diff --git a/dpilqr/decentralized.py b/dpilqr/distributed.py similarity index 92% rename from dpilqr/decentralized.py rename to dpilqr/distributed.py index 0f86ae8..ea3b9ad 100644 --- a/dpilqr/decentralized.py +++ b/dpilqr/distributed.py @@ -11,7 +11,6 @@ import itertools import logging -import multiprocessing as mp from time import perf_counter as pc import numpy as np @@ -23,8 +22,8 @@ g = 9.81 -def solve_decentralized(problem, X, U, radius, t_kill=None, pool=None, verbose=True, **kwargs): - """Solve the problem via decentralization into subproblems""" +def solve_distributed(problem, X, U, radius, pool=None, verbose=True, **kwargs): + """Solve the problem via division into subproblems""" x_dims = problem.game_cost.x_dims u_dims = problem.game_cost.u_dims @@ -64,8 +63,8 @@ def solve_decentralized(problem, X, U, radius, t_kill=None, pool=None, verbose=T if verbose: print(f"Problem {id_}: {graph[id_]}\nTook {Δt} seconds\n") - X_dec[:, i * n_states: (i + 1) * n_states] = Xi_agent - U_dec[:, i * n_controls: (i + 1) * n_controls] = Ui_agent + X_dec[:, i * n_states : (i + 1) * n_states] = Xi_agent + U_dec[:, i * n_controls : (i + 1) * n_controls] = Ui_agent solve_info[id_] = (Δt, graph[id_]) @@ -82,8 +81,8 @@ def solve_decentralized(problem, X, U, radius, t_kill=None, pool=None, verbose=T Δt = pc() - t0 if verbose: print(f"Problem {id_}: {graph[id_]}\nTook {Δt} seconds") - X_dec[:, i * n_states: (i + 1) * n_states] = Xi_agent - U_dec[:, i * n_controls: (i + 1) * n_controls] = Ui_agent + X_dec[:, i * n_states : (i + 1) * n_states] = Xi_agent + U_dec[:, i * n_controls : (i + 1) * n_controls] = Ui_agent # NOTE: This cannot be compared to the single-processed version due to # multi-processing overhead. @@ -96,7 +95,7 @@ def solve_decentralized(problem, X, U, radius, t_kill=None, pool=None, verbose=T return X_dec, U_dec, J_full, solve_info -def solve_rhc( # N is the length of the prediction horizon +def solve_rhc( problem, x0, N, @@ -142,7 +141,7 @@ def predicate(x, _): xi = x0.reshape(1, -1) X = xi.copy() # U = np.zeros((N, n_u)) - U = np.random.rand(N, n_u)*0.01 + U = np.random.rand(N, n_u) * 0.01 # U = np.tile([g, 0, 0], (N, n_agents)) centralized_solver = ilqrSolver(problem, N) @@ -164,7 +163,7 @@ def predicate(x, _): ) # print(f"Shape of X at each prediction horizon is{X.shape}") else: - X, U, J, solve_info = solve_decentralized( + X, U, J, solve_info = solve_distributed( problem, X, U, *args, verbose=False, **kwargs ) # print(f"Shape of X at each prediction horizon is{X.shape}") diff --git a/dpilqr/dynamics.py b/dpilqr/dynamics.py index a51f2ef..985b1f7 100644 --- a/dpilqr/dynamics.py +++ b/dpilqr/dynamics.py @@ -5,7 +5,6 @@ import abc import numpy as np -from scipy.constants import g from scipy.optimize import approx_fprime from scipy.integrate import solve_ivp import sympy as sym @@ -152,8 +151,8 @@ def f(self, x, u): nx = self.x_dims[0] nu = self.u_dims[0] for i, model in enumerate(self.submodels): - xn[i * nx: (i + 1) * nx] = model.f( - x[i * nx: (i + 1) * nx], u[i * nu: (i + 1) * nu] + xn[i * nx : (i + 1) * nx] = model.f( + x[i * nx : (i + 1) * nx], u[i * nu : (i + 1) * nu] ) return xn @@ -166,8 +165,8 @@ def __call__(self, x, u): nx = self.x_dims[0] nu = self.u_dims[0] for i, model in enumerate(self.submodels): - xn[i * nx: (i + 1) * nx] = model.__call__( - x[i * nx: (i + 1) * nx], u[i * nu: (i + 1) * nu] + xn[i * nx : (i + 1) * nx] = model.__call__( + x[i * nx : (i + 1) * nx], u[i * nu : (i + 1) * nu] ) return xn diff --git a/dpilqr/graphics.py b/dpilqr/graphics.py index 0556019..2f817d7 100644 --- a/dpilqr/graphics.py +++ b/dpilqr/graphics.py @@ -9,7 +9,7 @@ import networkx as nx import numpy as np -from decentralized.util import split_agents, compute_pairwise_distance +from dpilqr.util import split_agents, compute_pairwise_distance plt.rcParams.update( diff --git a/dpilqr/problem.py b/dpilqr/problem.py index 7891f57..f192923 100644 --- a/dpilqr/problem.py +++ b/dpilqr/problem.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -"""Logic to combine dynamics and cost in one framework to simplify decentralization""" +"""Logic to combine dynamics and cost in one framework to simplify distribution""" from time import perf_counter as pc @@ -34,7 +34,7 @@ def ids(self): return self.dynamics.ids.copy() def split(self, graph): - """Split up this centralized problem into a list of decentralized + """Split up this centralized problem into a list of distributed sub-problems. """ diff --git a/run/analysis.py b/run/analysis.py index 916ad01..2a7035e 100644 --- a/run/analysis.py +++ b/run/analysis.py @@ -27,7 +27,7 @@ QuadcopterDynamics6D, MultiDynamicalModel, ) -from dpilqr.decentralized import solve_rhc +from dpilqr.distributed import solve_rhc from dpilqr.problem import ilqrProblem from dpilqr.util import split_agents_gen, random_setup diff --git a/run/examples.py b/run/examples.py index c5366c1..c2ad661 100644 --- a/run/examples.py +++ b/run/examples.py @@ -16,7 +16,7 @@ import matplotlib.pyplot as plt from dpilqr import split_agents, plot_solve -import dpilqr as dec +import dpilqr import scenarios π = np.pi @@ -30,15 +30,15 @@ def single_unicycle(): x = np.array([-10, 10, 10, 0], dtype=float) x_goal = np.zeros((4, 1), dtype=float).T - dynamics = dec.UnicycleDynamics4D(dt) + dynamics = dpilqr.UnicycleDynamics4D(dt) Q = np.diag([1.0, 1, 0, 0]) Qf = 1000 * np.eye(Q.shape[0]) R = np.eye(2) - cost = dec.ReferenceCost(x_goal, Q, R, Qf) + cost = dpilqr.ReferenceCost(x_goal, Q, R, Qf) - prob = dec.ilqrProblem(dynamics, cost) - ilqr = dec.ilqrSolver(prob, N) + prob = dpilqr.ilqrProblem(dynamics, cost) + ilqr = dpilqr.ilqrSolver(prob, N) X, _, J = ilqr.solve(x) plt.clf() @@ -54,15 +54,15 @@ def single_quad6d(): x = np.array([2, 2, 0.5, 0, 0, 0], dtype=float) xf = np.zeros((6, 1), dtype=float).T - dynamics = dec.QuadcopterDynamics6D(dt) + dynamics = dpilqr.QuadcopterDynamics6D(dt) Q = np.eye(6) Qf = 100 * np.eye(Q.shape[0]) R = np.diag([0, 1, 1]) - cost = dec.ReferenceCost(xf, Q, R, Qf) + cost = dpilqr.ReferenceCost(xf, Q, R, Qf) - prob = dec.ilqrProblem(dynamics, cost) - ilqr = dec.ilqrSolver(prob, N) + prob = dpilqr.ilqrProblem(dynamics, cost) + ilqr = dpilqr.ilqrSolver(prob, N) X, _, J = ilqr.solve(x) @@ -97,23 +97,27 @@ def two_quads_one_human(): Rs = [R, R, R_human] Qfs = [Qf, Qf, Qf_human] - models = [dec.QuadcopterDynamics6D, dec.QuadcopterDynamics6D, dec.HumanDynamics6D] + models = [ + dpilqr.QuadcopterDynamics6D, + dpilqr.QuadcopterDynamics6D, + dpilqr.HumanDynamics6D, + ] ids = [100 + i for i in range(n_agents)] - dynamics = dec.MultiDynamicalModel( + dynamics = dpilqr.MultiDynamicalModel( [model(dt, id_) for id_, model in zip(ids, models)] ) goal_costs = [ - dec.ReferenceCost(xf_i, Qi, Ri, Qfi, id_) + dpilqr.ReferenceCost(xf_i, Qi, Ri, Qfi, id_) for xf_i, id_, x_dim, Qi, Ri, Qfi in zip( - dec.split_agents_gen(xf, x_dims), ids, x_dims, Qs, Rs, Qfs + dpilqr.split_agents_gen(xf, x_dims), ids, x_dims, Qs, Rs, Qfs ) ] - prox_cost = dec.ProximityCost(x_dims, radius, n_dims) - game_cost = dec.GameCost(goal_costs, prox_cost) + prox_cost = dpilqr.ProximityCost(x_dims, radius, n_dims) + game_cost = dpilqr.GameCost(goal_costs, prox_cost) - problem = dec.ilqrProblem(dynamics, game_cost) - solver = dec.ilqrSolver(problem, N) + problem = dpilqr.ilqrProblem(dynamics, game_cost) + solver = dpilqr.ilqrSolver(problem, N) U0 = np.c_[np.tile([g, 0, 0], (N, 2)), np.ones((N, n_controls))] X, _, J = solver.solve(x0, U0) @@ -122,7 +126,7 @@ def two_quads_one_human(): plot_solve(X, J, xf, x_dims, True, 3) plt.figure() - dec.plot_pairwise_distances(X, x_dims, n_dims, radius) + dpilqr.plot_pairwise_distances(X, x_dims, n_dims, radius) plt.show() @@ -138,7 +142,7 @@ def random_multiagent_simulation(): n_d = n_dims[0] - x0, xf = dec.random_setup( + x0, xf = dpilqr.random_setup( n_agents, n_states, is_rotation=False, @@ -151,7 +155,7 @@ def random_multiagent_simulation(): x_dims = [n_states] * n_agents u_dims = [n_controls] * n_agents - dec.eyeball_scenario(x0, xf, n_agents, n_states) + dpilqr.eyeball_scenario(x0, xf, n_agents, n_states) plt.show() dt = 0.05 @@ -160,8 +164,8 @@ def random_multiagent_simulation(): tol = 1e-6 ids = [100 + i for i in range(n_agents)] - model = dec.UnicycleDynamics4D - dynamics = dec.MultiDynamicalModel([model(dt, id_) for id_ in ids]) + model = dpilqr.UnicycleDynamics4D + dynamics = dpilqr.MultiDynamicalModel([model(dt, id_) for id_ in ids]) Q = np.eye(4) R = np.eye(2) @@ -169,21 +173,21 @@ def random_multiagent_simulation(): radius = 0.5 goal_costs = [ - dec.ReferenceCost(xf_i, Q.copy(), R.copy(), Qf.copy(), id_) + dpilqr.ReferenceCost(xf_i, Q.copy(), R.copy(), Qf.copy(), id_) for xf_i, id_, x_dim, u_dim in zip( - dec.split_agents_gen(xf, x_dims), ids, x_dims, u_dims + dpilqr.split_agents_gen(xf, x_dims), ids, x_dims, u_dims ) ] - prox_cost = dec.ProximityCost(x_dims, radius, n_dims) + prox_cost = dpilqr.ProximityCost(x_dims, radius, n_dims) goal_costs = [ - dec.ReferenceCost(xf_i, Q.copy(), R.copy(), Qf.copy(), id_) + dpilqr.ReferenceCost(xf_i, Q.copy(), R.copy(), Qf.copy(), id_) for xf_i, id_ in zip(split_agents(xf.T, x_dims), ids) ] - prox_cost = dec.ProximityCost(x_dims, radius, n_dims) - game_cost = dec.GameCost(goal_costs, prox_cost) + prox_cost = dpilqr.ProximityCost(x_dims, radius, n_dims) + game_cost = dpilqr.GameCost(goal_costs, prox_cost) - problem = dec.ilqrProblem(dynamics, game_cost) - solver = dec.ilqrSolver(problem, N) + problem = dpilqr.ilqrProblem(dynamics, game_cost) + solver = dpilqr.ilqrSolver(problem, N) X, _, J = solver.solve(x0, tol=tol, t_kill=None) @@ -191,11 +195,11 @@ def random_multiagent_simulation(): plot_solve(X, J, xf.T, x_dims, True, n_d) plt.figure() - dec.plot_pairwise_distances(X, x_dims, n_dims, radius) + dpilqr.plot_pairwise_distances(X, x_dims, n_dims, radius) plt.show() - dec.make_trajectory_gif(f"{n_agents}-unicycles.gif", X, xf, x_dims, radius) + dpilqr.make_trajectory_gif(f"{n_agents}-unicycles.gif", X, xf, x_dims, radius) def main(): diff --git a/tests/test_cost.py b/tests/test_cost.py index 89642e7..39e8535 100644 --- a/tests/test_cost.py +++ b/tests/test_cost.py @@ -1,37 +1,33 @@ #!/usr/bin/env python -"""Unit testing for the cost module - -TODO - -""" +"""Unit testing for the cost module""" import unittest import numpy as np -import decentralized as dec +import dpilqr class TestProximityCost(unittest.TestCase): def test_single(self): - cost = dec.ProximityCost([2], 10.0)([1, 2]) + cost = dpilqr.ProximityCost([2], 10.0)([1, 2]) self.assertAlmostEqual(cost, 0.0) def test_call_2(self): r = 10.0 x = np.array([0, 0, 0, 1, 2, 0]) - cost = dec.ProximityCost([3, 3], r)(x) + cost = dpilqr.ProximityCost([3, 3], r)(x) expected = (np.hypot(1, 2) - r) ** 2 self.assertAlmostEqual(cost, expected) def test_quadraticize_3(self): x = np.arange(9) - cost = dec.ProximityCost([3, 3, 3], 10.0) + cost = dpilqr.ProximityCost([3, 3, 3], 10.0) Lx, _, Lxx, *_ = cost.quadraticize(x) u = np.zeros(6) - Lx_diff, _, Lxx_diff, *_ = dec.quadraticize_finite_difference( + Lx_diff, _, Lxx_diff, *_ = dpilqr.quadraticize_finite_difference( cost.__call__, x, u, False ) @@ -48,7 +44,7 @@ def setUp(self): self.R = np.eye(self.m) self.Qf = np.diag([1, 1, 0]) - self.ref_cost = dec.ReferenceCost(self.xf, self.Q, self.R, self.Qf) + self.ref_cost = dpilqr.ReferenceCost(self.xf, self.Q, self.R, self.Qf) self.x0 = np.random.randint(0, 10, (self.n,)) self.u = np.random.randint(0, 10, (self.m,)) @@ -87,8 +83,8 @@ def test_single(self): xf = np.zeros(4) Q = np.eye(4) R = np.eye(2) - player_cost = dec.ReferenceCost(xf, Q, R) - game_cost = dec.GameCost([player_cost]) + player_cost = dpilqr.ReferenceCost(xf, Q, R) + game_cost = dpilqr.GameCost([player_cost]) x = np.random.rand(4) u = np.random.rand(2) @@ -98,7 +94,7 @@ def test_single(self): # Approximately validate with finite difference. Lx, Lu, Lxx, Luu, _ = game_cost.quadraticize(x, u) - Lx_diff, Lu_diff, Lxx_diff, Luu_diff, _ = dec.quadraticize_finite_difference( + Lx_diff, Lu_diff, Lxx_diff, Luu_diff, _ = dpilqr.quadraticize_finite_difference( game_cost.__call__, x, u ) @@ -114,15 +110,15 @@ def test_multi(self): x_dims = [3, 3, 3] radius = 5.0 player_costs = [ - dec.ReferenceCost(xfi, Q, R) for xfi in dec.split_agents(xf, x_dims) + dpilqr.ReferenceCost(xfi, Q, R) for xfi in dpilqr.split_agents(xf, x_dims) ] - prox_cost = dec.ProximityCost(x_dims, radius) - game_cost = dec.GameCost(player_costs, prox_cost) + prox_cost = dpilqr.ProximityCost(x_dims, radius) + game_cost = dpilqr.GameCost(player_costs, prox_cost) x = 10 * np.random.randn(9) u = np.random.randn(6) Lx, Lu, Lxx, Luu, _ = game_cost.quadraticize(x, u) - Lx_diff, Lu_diff, Lxx_diff, Luu_diff, _ = dec.quadraticize_finite_difference( + Lx_diff, Lu_diff, Lxx_diff, Luu_diff, _ = dpilqr.quadraticize_finite_difference( game_cost.__call__, x, u ) diff --git a/tests/test_dynamics.py b/tests/test_dynamics.py index 86edbf8..01c427f 100644 --- a/tests/test_dynamics.py +++ b/tests/test_dynamics.py @@ -6,7 +6,7 @@ import numpy as np -import dpilqr as dec +import dpilqr class _TestDynamics: @@ -18,7 +18,7 @@ def _test_integrate(self, x0, u, X_truth): def _test_linearize(self, x0, u, **kwargs): A, B = self.model.linearize(x0, u) - A_diff, B_diff = dec.linearize_finite_difference(self.model.__call__, x0, u) + A_diff, B_diff = dpilqr.linearize_finite_difference(self.model.__call__, x0, u) self.assertTrue(np.allclose(A, A_diff, **kwargs)) self.assertTrue(np.allclose(B, B_diff, **kwargs)) @@ -26,7 +26,7 @@ def _test_linearize(self, x0, u, **kwargs): class TestDoubleInt4D(_TestDynamics, unittest.TestCase): def setUp(self): - self.model = dec.DoubleIntDynamics4D(0.5) + self.model = dpilqr.DoubleIntDynamics4D(0.5) def test_call(self): x = np.array([0.0, 2, 0, -2]) @@ -44,7 +44,7 @@ def test_integrate(self): class TestCarDynamics3D(_TestDynamics, unittest.TestCase): def setUp(self): - self.model = dec.CarDynamics3D(0.5) + self.model = dpilqr.CarDynamics3D(0.5) def test_call(self): x0 = np.array([0, 0, np.pi / 4]) @@ -63,7 +63,7 @@ def test_linearize(self): class TestUnicycle4D(_TestDynamics, unittest.TestCase): def setUp(self): - self.model = dec.UnicycleDynamics4D(1.0) + self.model = dpilqr.UnicycleDynamics4D(1.0) def test_straight(self): x0 = np.zeros(4) @@ -104,7 +104,7 @@ def test_linearize(self): class TestBikeDynamics5D(_TestDynamics, unittest.TestCase): def setUp(self): - self.model = dec.BikeDynamics5D(0.5) + self.model = dpilqr.BikeDynamics5D(0.5) def test_linearize(self): x = np.random.rand(5)