From 9c9470c68e1101f34d07f5fcca37febe00864ed1 Mon Sep 17 00:00:00 2001 From: Zach Williams Date: Thu, 2 Mar 2023 18:00:00 -0600 Subject: [PATCH] Quads + humans checkpoint --- dpilqr/__init__.py | 1 + dpilqr/bbdynamics.cpp | 25 ++++++++++++- dpilqr/bbdynamicswrap.pyx | 10 +++++ dpilqr/control.py | 1 + dpilqr/distributed.py | 10 ++++- dpilqr/dynamics.py | 6 +++ dpilqr/graphics.py | 24 +++++++----- dpilqr/problem.py | 2 +- scripts/examples.py | 78 +++++++++++++++++++++++++++++++++++++-- scripts/scenarios.py | 50 ++++++++++++++++++++----- 10 files changed, 182 insertions(+), 25 deletions(-) diff --git a/dpilqr/__init__.py b/dpilqr/__init__.py index f79e2b5..53c1bee 100644 --- a/dpilqr/__init__.py +++ b/dpilqr/__init__.py @@ -21,6 +21,7 @@ DoubleIntDynamics6D, DynamicalModel, HumanDynamics6D, + HumanDynamicsLin6D, MultiDynamicalModel, QuadcopterDynamics6D, QuadcopterDynamics12D, diff --git a/dpilqr/bbdynamics.cpp b/dpilqr/bbdynamics.cpp index ee00ff2..70d69b6 100644 --- a/dpilqr/bbdynamics.cpp +++ b/dpilqr/bbdynamics.cpp @@ -318,7 +318,6 @@ static void f_human_6d(double x[], double u[], double x_dot[]) /* x: = [px, py , pz, v, 0, 0] u: = [theta a] - */ x_dot[0] = x[3] * cos(u[0]); @@ -391,6 +390,30 @@ void linearize_human_6d(double x[], double u[], double dt, double A[], double B[ euler_method_discretization(dt, A, B, 6, 3); } +static void f_human_lin_6d(double x[], double u[], double x_dot[]) +{ + /* x: [px, py, pz, vx, vy, vz] + u: [ax, ay, az] + + NOTE: 2D Double integrator with constant heigh. + */ + x_dot[0] = x[3]; + x_dot[1] = x[4]; + x_dot[2] = 0.0; + x_dot[3] = u[0]; + x_dot[4] = u[1]; + x_dot[5] = 0.0; +} + +static void linearize_human_lin_6d(double dt, double A[], double B[]) +{ + linearize_double_int_6d(dt, A, B); + + // Turn off changes along z. + A[17] = 0; + B[17] = 0; +} + static void f_quad_6d(double x[], double u[], double x_dot[]) { /* x: [px, py, pz, vx, vy, vz] diff --git a/dpilqr/bbdynamicswrap.pyx b/dpilqr/bbdynamicswrap.pyx index f32aa7f..a31e304 100644 --- a/dpilqr/bbdynamicswrap.pyx +++ b/dpilqr/bbdynamicswrap.pyx @@ -12,6 +12,7 @@ class Model(Enum): Unicycle4D = auto() Quadcopter6D = auto() Human6D = auto() + HumanLin6D = auto() Quadcopter12D = auto() @@ -38,6 +39,9 @@ cdef extern from "bbdynamics.cpp": void f_human_6d(double x[], double u[], double x_dot[]) void linearize_human_6d(double x[], double u[], double dt, double A[], double B[]) + void f_human_lin_6d(double x[], double u[], double x_dot[]) + void linearize_human_lin_6d(double dt, double A[], double B[]) + void f_quad_6d(double x[], double u[], double x_dot[]) void linearize_quad_6d(double x[], double u[], double dt, double A[], double B[]) @@ -75,6 +79,8 @@ def f(x, u, model): f = f_unicycle_4d elif model is Model.Human6D: f = f_human_6d + elif model is Model.HumanLin6D: + f = f_human_lin_6d elif model is Model.Quadcopter6D: f = f_quad_6d elif model is Model.Quadcopter12D: @@ -106,6 +112,8 @@ def integrate(x, u, double dt, model): f = f_unicycle_4d elif model is Model.Human6D: f = f_human_6d + elif model is Model.HumanLin6D: + f = f_human_lin_6d elif model is Model.Quadcopter6D: f = f_quad_6d elif model is Model.Quadcopter12D: @@ -146,6 +154,8 @@ def linearize(x, u, double dt, model): linearize_unicycle_4d(&x_view[0], &u_view[0], dt, &A_view[0], &B_view[0]) elif model is Model.Human6D: linearize_human_6d(&x_view[0], &u_view[0], dt, &A_view[0], &B_view[0]) + elif model is Model.HumanLin6D: + linearize_human_lin_6d(dt, &A_view[0], &B_view[0]) elif model is Model.Quadcopter6D: linearize_quad_6d(&x_view[0], &u_view[0], dt, &A_view[0], &B_view[0]) elif model is Model.Quadcopter12D: diff --git a/dpilqr/control.py b/dpilqr/control.py index e333fcc..2e7f11e 100644 --- a/dpilqr/control.py +++ b/dpilqr/control.py @@ -195,6 +195,7 @@ def solve(self, x0, U=None, n_lqr_iter=50, tol=1e-3, t_kill=None, verbose=True): if not accept: # DBG: bail out since regularization doesn't seem to help. + break if verbose: print("Failed line search, giving up.") break diff --git a/dpilqr/distributed.py b/dpilqr/distributed.py index ea3b9ad..87648a9 100644 --- a/dpilqr/distributed.py +++ b/dpilqr/distributed.py @@ -22,7 +22,7 @@ g = 9.81 -def solve_distributed(problem, X, U, radius, pool=None, verbose=True, **kwargs): +def solve_distributed(problem, X, U, radius, ignore_ids=None, pool=None, verbose=True, **kwargs): """Solve the problem via division into subproblems""" x_dims = problem.game_cost.x_dims @@ -35,6 +35,9 @@ def solve_distributed(problem, X, U, radius, pool=None, verbose=True, **kwargs): ids = problem.ids solve_info = {} + if ignore_ids and any(id_ not in ids for id_ in ignore_ids): + raise ValueError(f"Some of {ignore_ids} not in {ids}.") + # Compute interaction graph based on relative distances. graph = define_inter_graph_threshold(X, radius, x_dims, ids) if verbose: @@ -53,6 +56,11 @@ def solve_distributed(problem, X, U, radius, pool=None, verbose=True, **kwargs): for i, (subproblem, x0i, Ui, id_) in enumerate( zip(problem.split(graph), x0_split, U_split, ids) ): + if id_ in ignore_ids: + if verbose: + solve_info[id_] = (0.0, [id_]) + print(f"Ignoring subproblem {id_}...") + continue t0 = pc() Xi_agent, Ui_agent, id_ = solve_subproblem( diff --git a/dpilqr/dynamics.py b/dpilqr/dynamics.py index 7b54af0..23dd1e1 100644 --- a/dpilqr/dynamics.py +++ b/dpilqr/dynamics.py @@ -244,6 +244,12 @@ def __init__(self, dt, *args, **kwargs): self.model = Model.Human6D +class HumanDynamicsLin6D(CppModel): + def __init__(self, dt, *args, **kwargs): + super().__init__(6, 3, dt, *args, **kwargs) + self.model = Model.HumanLin6D + + # TODO: Consider making a CPP model for these two: class BikeDynamics5D(SymbolicModel): def __init__(self, dt, *args, **kwargs): diff --git a/dpilqr/graphics.py b/dpilqr/graphics.py index 5248c25..9ed7862 100644 --- a/dpilqr/graphics.py +++ b/dpilqr/graphics.py @@ -107,6 +107,7 @@ def plot_solve(X, J, x_goal, x_dims=None, color_agents=False, n_d=2, ax=None): N = X.shape[0] n = np.arange(N) + cm = plt.cm.Set2 X_split = split_agents(X, x_dims) x_goal_split = split_agents(x_goal.reshape(1, -1), x_dims) @@ -115,22 +116,27 @@ def plot_solve(X, J, x_goal, x_dims=None, color_agents=False, n_d=2, ax=None): c = n if n_d == 2: if color_agents: - c = plt.cm.tab10.colors[i] - ax.plot(Xi[:, 0], Xi[:, 1], c=c, lw=5, zorder=1) + c = cm.colors[i] + ax.plot(Xi[:, 0], Xi[:, 1], c=c, lw=5) else: ax.scatter(Xi[:, 0], Xi[:, 1], c=c) - ax.scatter(Xi[0, 0], Xi[0, 1], 80, "g", "x", label="$x_0$") + ax.scatter(Xi[0, 0], Xi[0, 1], 80, "g", "d", label="$x_0$") ax.scatter(xg[0, 0], xg[0, 1], 80, "r", "x", label="$x_f$") else: if color_agents: - c = [plt.cm.tab10.colors[i]] * Xi.shape[0] - ax.scatter(Xi[:, 0], Xi[:, 1], Xi[:, 2], c=c) + # c = [cm.colors[i]] * Xi.shape[0] + c = cm.colors[i] + ax.plot(Xi[:, 0], Xi[:, 1], Xi[:, 2], c=c, lw=4) ax.scatter( - Xi[0, 0], Xi[0, 1], Xi[0, 2], s=80, c="g", marker="x", label="$x_0$" - ) + Xi[0, 0], Xi[0, 1], Xi[0, 2], + s=50, c="w", marker="d", edgecolors="k", label="$x_0$") ax.scatter( - xg[0, 0], xg[0, 1], xg[0, 2], s=80, c="r", marker="x", label="$x_f$" - ) + xg[0, 0], xg[0, 1], xg[0, 2], + s=50, c="k", marker="x", label="$x_f$") + ax.scatter( + Xi[-1, 0], Xi[-1, 1], Xi[-1,2], + s=50, color=c, marker="o", edgecolors="k") + plt.margins(0.1) plt.title(f"Final Cost: {J:.3g}") diff --git a/dpilqr/problem.py b/dpilqr/problem.py index f192923..2860b72 100644 --- a/dpilqr/problem.py +++ b/dpilqr/problem.py @@ -68,7 +68,7 @@ def selfish_warmstart(self, x0, N): print("=" * 80 + "\nComputing warmstart...") # Split out the full problem into separate problems for each agent. - x0 = x0.reshape(1, -1) + x0 = x0.reshape(-1, 1) selfish_graph = {id_: [id_] for id_ in self.ids} subproblems = self.split(selfish_graph) diff --git a/scripts/examples.py b/scripts/examples.py index 829d2e0..4bf6990 100644 --- a/scripts/examples.py +++ b/scripts/examples.py @@ -83,7 +83,7 @@ def two_quads_one_human(): N = 50 radius = 0.3 - x0, xf = scenarios.two_quads_one_human_setup() + x0, xf = scenarios.q2h1_passthrough_setup() Q = np.diag([1, 1, 1, 5, 5, 5]) R = np.diag([1, 1, 1]) @@ -259,14 +259,86 @@ def _3d_integrators(): # dpilqr.make_trajectory_gif(f"{n_agents}-quads.gif", X, xf, x_dims, radius) +def nquads_mhumans(): + + n = 2 + m = 2 + n_agents = n + m + n_states = 6 + n_controls = 3 + + x_dims = [n_states] * n_agents + n_dims = [3, 3, 2, 2] + # n_dims = [3, 3, 3, 3] + + dt = 0.05 + N = 60 + radius = 1.0 + + x0, xf = scenarios.q3h2_qcross() + x0, xf = scenarios.q2h2_hcross() + + Q = np.eye(6) + R = 0.1*np.eye(3) + Qf = 1e4 * np.eye(n_states) + + Q_human = Q.copy() + R_human = R.copy() + Qf_human = Qf.copy() + # Q_human = np.diag([1, 1, 0, 0, 0, 0]) + # R_human = np.diag([1, 1, 1]) + # Qf_human = 1e3 * np.eye(Q.shape[0]) + + Qs = [Q] * n + [Q_human] * m + Rs = [R] * n + [R_human] * m + Qfs = [Qf] * n + [Qf_human] * m + + models = [dpilqr.QuadcopterDynamics6D] * n + [dpilqr.HumanDynamicsLin6D] * m + # models = [dpilqr.QuadcopterDynamics6D] * n + [dpilqr.QuadcopterDynamics6D] * m + ids = [100 + i for i in range(n_agents)] + dynamics = dpilqr.MultiDynamicalModel( + [model(dt, id_) for id_, model in zip(ids, models)] + ) + + goal_costs = [ + dpilqr.ReferenceCost(xf_i, Qi, Ri, Qfi, id_) + for xf_i, id_, x_dim, Qi, Ri, Qfi in zip( + dpilqr.split_agents_gen(xf, x_dims), ids, x_dims, Qs, Rs, Qfs + ) + ] + prox_cost = dpilqr.ProximityCost(x_dims, radius, n_dims) + game_cost = dpilqr.GameCost(goal_costs, prox_cost) + + h_ids = ids[-2:] + problem = dpilqr.ilqrProblem(dynamics, game_cost) + solver = dpilqr.ilqrSolver(problem, N) + + # U0 = np.zeros((N, n_controls*n_agents)) + U0 = problem.selfish_warmstart(x0, N) + # X, _, J = solver.solve(x0, U0) + X, _, J, solve_info = dpilqr.solve_distributed(problem, x0.T, U0, radius, h_ids, pool=None, verbose=True) + igraph = {k: v[1] for k, v in solve_info.items()} + dpilqr.plot_interaction_graph(igraph) + + plt.figure() + plot_solve(X, J, xf, x_dims, True, 3) + plt.gca().set_zlim([0, 2]) + + plt.figure() + dpilqr.plot_pairwise_distances(X, x_dims, n_dims, radius) + + plt.show() + + + def main(): # single_unicycle() # single_quad6d() # two_quads_one_human() # random_multiagent_simulation() - _3d_integrators() - + # _3d_integrators() + nquads_mhumans() if __name__ == "__main__": main() diff --git a/scripts/scenarios.py b/scripts/scenarios.py index 7db479e..f39c09e 100644 --- a/scripts/scenarios.py +++ b/scripts/scenarios.py @@ -70,16 +70,6 @@ def paper_setup_7_quads(): return x0, xf -def two_quads_one_human_setup(): - x0 = np.array([[-1.5, 0.1, 1, 0, 0, 0, - 1.5, 0, 1, 0, 0, 0, - 0, -1, 1.5, 0, 0, 0]]).T - xf = np.array([[1.5, 0, 2, 0, 0, 0, - -1.5, 0, 2, 0, 0, 0, - 0.0, 2, 1.5, 0, 0, 0]]).T - return x0, xf - - def four_quads_exchange(): x0 = np.array([[0.0, 0, 1, 0, 0, 0, 1.0, 0, 1, 0, 0, 0, @@ -149,4 +139,44 @@ def five_quads_figure1(): 1.5, 0.4, 1.0, 0, 0, 0, 1.0, 1.0, 1.0, 0, 0, 0, ]]).T + return x0, xf + + +def q2h1_passthrough(): + x0 = np.array([[-1.5, 0.1, 1, 0, 0, 0, + 1.5, 0, 1, 0, 0, 0, + 0, -1, 1.5, 0, 0, 0]]).T + xf = np.array([[1.5, 0, 2, 0, 0, 0, + -1.5, 0, 2, 0, 0, 0, + 0.0, 2, 1.5, 0, 0, 0]]).T + return x0, xf + + +def q3h2_qcross(): + x0 = np.array([[-1.5, 1, 0.95, 0, 0, 0, + 1.5, 0.8, 1.05, 0, 0, 0, + 0, -2, 0.9, 0, 0, 0, + -1.2, -1.1, 1, 0, 0, 0, + +1.4, -0.9, 1, 0, 0, 0 + ]]).T + xf = np.array([[1.5, 0.8, 1.05, 0, 0, 0, + -1.5, 1.1, 0.9, 0, 0, 0, + 0, 0, 1.1, 0, 0, 0, + +1.3, -0.95, 1, 0, 0, 0, + -1.0, -1.05, 1, 0, 0, 0, + ]]).T + return x0, xf + + +def q2h2_hcross(): + x0 = np.array([[ + 1.0, 0.4, 0.5, 0, 0.5, 0, + 0.1, -1.2, 0.8, 0.5, 0, 0, + -1.0, 0.2, 1, 0, 0, 0, + 0.2, 1.0, 1, 0, 0, 0]]).T + xf = np.array([[ + -1.0, 1.2, 1, 0, 0, 0, + 0.3, 1.05, 1, 0, 0, 0, + 0.5, 0.0, 1, 0, 0, 0, + 0.0, -1.2, 1.2, 0, 0, 0]]).T return x0, xf \ No newline at end of file