Quads + humans checkpoint

dpilqr/__init__.py

@@ -21,6 +21,7 @@
     DoubleIntDynamics6D,
     DynamicalModel,
     HumanDynamics6D,
+    HumanDynamicsLin6D,
     MultiDynamicalModel,
     QuadcopterDynamics6D,
     QuadcopterDynamics12D,

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]

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:

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

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(

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):

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}")

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)
 

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()

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