def plot_win_curve(n=-1, num_pts=20):
        global curves_when_i_win
        if n > len(curves_when_i_win) - 1 or n < 0:
            print(
                "n bigger than num curves or equal to -1, plotting last curve")
            n = len(curves_when_i_win) - 1
        c0, dc0, c_end, dc_end, t_max, c_of_t, dc_of_t, ddc_of_t, H, h, p, N, dl_of_t, L_of_t = curves_when_i_win[
            n]
        print("c0 ", c0)
        print("Is c0 stable ? ", check_static_eq(H, h, mass, c0, g_vector))
        print("Is end stable ? ",
              check_static_eq(H, h, mass, c_of_t(t_max), g_vector))

        w = np.zeros(6)
        w[2] = -mass * 9.81
        w[3:] = mass * np.cross(c_of_t(t_max), g_vector)
        print('max ', np.max(np.dot(H, w) - h))

        X_MIN = np.min(p[:, 0])
        X_MAX = np.max(p[:, 0])
        X_MIN -= 0.1 * (X_MAX - X_MIN)
        X_MAX += 0.1 * (X_MAX - X_MIN)
        Y_MIN = np.min(p[:, 1])
        Y_MAX = np.max(p[:, 1])
        print("Is XMIN ? ", X_MIN)
        print("Is XMAX ? ", X_MAX)
        print("Is YMIN ? ", Y_MIN)
        print("Is YMAX ? ", Y_MAX)
        delta = t_max / float(num_pts)
        num_pts += 1
        fig = plt.figure()
        ax = fig.add_subplot(221, projection='3d')
        # ax = fig.add_subplot(221)
        __plot_3d_points(ax, [c_of_t(i * delta) for i in range(num_pts)])
        __plot_3d_points(
            ax, [c0 + (c_end - c0) * i * delta for i in range(num_pts)], c="y")
        plot_support_polygon(H, h, p, N, ax, c0)
        ax = fig.add_subplot(222, projection='3d')
        __plot_3d_points(ax, [dc_of_t(i * delta) for i in range(num_pts)])
        __plot_3d_points(
            ax, [dc0 + (dc_end - dc0) * i * delta for i in range(num_pts)],
            c="y")
        ax = fig.add_subplot(223, projection='3d')
        # __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)])
        # ax = fig.add_subplot(224, projection='3d')
        __plot_3d_points(ax, [L_of_t(i * delta) for i in range(num_pts)],
                         c="y")
        # __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)], c = "y")
        ax = fig.add_subplot(224, projection='3d')
        __plot_3d_points(ax, [dl_of_t(i * delta) for i in range(num_pts)])
        # ax = fig.add_subplot(121, projection='3d')
        # __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)])
        # ax = fig.add_subplot(122, projection='3d')
        # __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)])
        # print("cross product ", X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) ) / norm(X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) )))
        # print("init acceleration ", ddc_of_t(0))
        print("init velocity ", dc_of_t(0))
        print("end velocity ", dc_of_t(t_max))
 def plot_support_polygon(H,h,p,N,ax, c0):
     from pinocchio_inv_dyn.multi_contact.utils import generate_contacts, find_static_equilibrium_com, compute_GIWC, compute_support_polygon
     #~ (H,h) = compute_GIWC(p, N, mu);        
     global mass
     global g_vector
     (B_sp, b_sp) = compute_support_polygon(H, h, mass, g_vector, eliminate_redundancies=False);
     X_MIN = np.min(p[:,0]);
     X_MAX = np.max(p[:,0]);
     X_MIN -= 0.5*(X_MAX-X_MIN);
     X_MAX += 0.5*(X_MAX-X_MIN);
     Y_MIN = np.min(p[:,1]);
     Y_MAX = np.max(p[:,1]);
     Y_MIN -= 0.5*(Y_MAX-Y_MIN);
     Y_MAX += 0.5*(Y_MAX-Y_MIN);
     num_steps = 50
     dx = (X_MAX - X_MIN) / float(num_steps)
     dy = (Y_MAX - Y_MIN) / float(num_steps)
     #~ points = [(X_MIN + dx * i, Y_MAX + dy * j, 0.) for i in range(num_steps+1) for j in range(num_steps+1) if check_static_eq(H, h, mass, array([X_MIN + dx * i, Y_MAX + dy * j,0.]), g_vector) ]
     #~ points = [c0]+[[X_MIN + dx * i, Y_MIN + dy * j, -0.5] for i in range(num_steps+1) for j in range(num_steps+1) if check_static_eq(H, h, mass, [X_MIN + dx * i, Y_MAX + dy * j,0.], g_vector)]
     points = [c0]+[[X_MIN + dx * i, Y_MIN + dy * j, 0] for i in range(num_steps+1) for j in range(num_steps+1) ]
     pts2  = []
     for pt in points:
         if check_static_eq(H, h, mass, pt, g_vector):
             pts2 += [pt]
     __plot_3d_points(ax, pts2, c="r")   
     #~ __plot_3d_points(ax, points2, c="r")   
     __plot_3d_points(ax, p, c="r")