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