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