def test(N_CONTACTS=2, solver='qpoases', verb=0): from pinocchio_inv_dyn.multi_contact.utils import generate_contacts, compute_GIWC, find_static_equilibrium_com, can_I_stop DO_PLOTS = False PLOT_3D = False mass = 75 # mass of the robot mu = 0.5 # friction coefficient lx = 0.1 # half foot size in x direction ly = 0.07 # half foot size in y direction #First, generate a contact configuration CONTACT_POINT_UPPER_BOUNDS = [0.5, 0.5, 0.5] CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0] gamma = atan(mu) # half friction cone angle RPY_LOWER_BOUNDS = [-2 * gamma, -2 * gamma, -pi] RPY_UPPER_BOUNDS = [+2 * gamma, +2 * gamma, +pi] MIN_CONTACT_DISTANCE = 0.3 g_vector = np.array([0., 0., -9.81]) X_MARG = 0.07 Y_MARG = 0.07 succeeded = False while (not succeeded): (p, N) = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, False) X_LB = np.min(p[:, 0] - X_MARG) X_UB = np.max(p[:, 0] + X_MARG) Y_LB = np.min(p[:, 1] - Y_MARG) Y_UB = np.max(p[:, 1] + Y_MARG) Z_LB = np.min(p[:, 2] - 0.05) Z_UB = np.max(p[:, 2] + 1.5) (H, h) = compute_GIWC(p, N, mu, False) (succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h) dc0 = np.random.uniform(-1, 1, size=3) # dc0[:] = 0; if (DO_PLOTS): f, ax = plut.create_empty_figure() for j in range(p.shape[0]): ax.scatter(p[j, 0], p[j, 1], c='k', s=100) ax.scatter(c0[0], c0[1], c='r', s=100) com_x = np.zeros(2) com_y = np.zeros(2) com_x[0] = c0[0] com_y[0] = c0[1] com_x[1] = c0[0] + dc0[0] com_y[1] = c0[1] + dc0[1] ax.plot(com_x, com_y, color='b') plt.axis([X_LB, X_UB, Y_LB, Y_UB]) plt.title('Contact Points and CoM position') if (PLOT_3D): fig = plt.figure(figsize=plt.figaspect(0.5) * 1.5) ax = fig.gca(projection='3d') line_styles = ["b", "r", "c", "g"] ss = [0.05, 0.1, 0.15, 0.2, 0.25, 0.3] ax.scatter(c0[0], c0[1], c0[2], c='k', marker='o') for i in range(p.shape[0]): ax.scatter(p[i, 0], p[i, 1], p[i, 2], c=line_styles[i % len(line_styles)], marker='o') for s in ss: ax.scatter(p[i, 0] + s * N[i, 0], p[i, 1] + s * N[i, 1], p[i, 2] + s * N[i, 2], c=line_styles[i % len(line_styles)], marker='x') for s in ss: ax.scatter(c0[0] + s * dc0[0], c0[1] + s * dc0[1], c0[2] + s * dc0[2], c='k', marker='x') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') if (verb > 0): print "p:", p.T print "N", N.T print "c", c0.T print "dc", dc0.T stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver) try: res = stabilitySolver.can_I_stop() except Exception as e: print "\n *** New algorithm failed:", e, "\nRe-running the algorithm with high verbosity\n" stabilitySolver._verb = 1 try: res = stabilitySolver.can_I_stop() except Exception as e: raise try: (has_stopped2, c_final2, dc_final2) = can_I_stop(c0, dc0, p, N, mu, mass, 1.0, 100, verb=verb, DO_PLOTS=DO_PLOTS) if ((res.is_stable != has_stopped2) or not np.allclose(res.c, c_final2, atol=1e-3) or not np.allclose(res.dc, dc_final2, atol=1e-3)): print "\nERROR: the two algorithms gave different results!" print "New algorithm:", res.is_stable, res.c, res.dc print "Old algorithm:", has_stopped2, c_final2, dc_final2 print "Errors:", norm(res.c - c_final2), norm(res.dc - dc_final2), "\n" except Exception as e: print "\n\n *** Old algorithm failed: ", e print "Results of new algorithm is", res.is_stable, "c0", c0, "dc0", dc0, "cFinal", res.c, "dcFinal", res.dc, "\n" return (stabilitySolver._computationTime, stabilitySolver._outerIterations, stabilitySolver._innerIterations)
def test(): np.set_printoptions(precision=1, suppress=True, linewidth=100) DO_PLOTS = False PLOT_3D = False mass = 75 # mass of the robot g_vector = np.array([0, 0, -9.81]) mu = 0.3 # friction coefficient lx = 0.1 # half foot size in x direction ly = 0.07 # half foot size in y direction USE_DIAGONAL_GENERATORS = True GENERATE_QUASI_FLAT_CONTACTS = True #First, generate a contact configuration CONTACT_POINT_UPPER_BOUNDS = [0.5, 0.5, 0.0] CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0] gamma = atan(mu) # half friction cone angle RPY_LOWER_BOUNDS = [-0 * gamma, -0 * gamma, -pi] RPY_UPPER_BOUNDS = [+0 * gamma, +0 * gamma, +pi] MIN_CONTACT_DISTANCE = 0.3 N_CONTACTS = 2 X_MARG = 0.07 Y_MARG = 0.07 succeeded = False while (succeeded == False): (p, N) = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, GENERATE_QUASI_FLAT_CONTACTS) X_LB = np.min(p[:, 0] - X_MARG) X_UB = np.max(p[:, 0] + X_MARG) Y_LB = np.min(p[:, 1] - Y_MARG) Y_UB = np.max(p[:, 1] + Y_MARG) Z_LB = np.min(p[:, 2] - 0.05) Z_UB = np.max(p[:, 2] + 1.5) (H, h) = compute_GIWC(p, N, mu, False, USE_DIAGONAL_GENERATORS) (succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h) dc0 = np.random.uniform(-1, 1, size=3) dc0[2] = 0 if (False and DO_PLOTS): f, ax = plut.create_empty_figure() for j in range(p.shape[0]): ax.scatter(p[j, 0], p[j, 1], c='k', s=100) ax.scatter(c0[0], c0[1], c='r', s=100) com_x = np.zeros(2) com_y = np.zeros(2) com_x[0] = c0[0] com_y[0] = c0[1] com_x[1] = c0[0] + dc0[0] com_y[1] = c0[1] + dc0[1] ax.plot(com_x, com_y, color='b') plt.axis([X_LB, X_UB, Y_LB, Y_UB]) plt.title('Contact Points and CoM position') if (PLOT_3D): fig = plt.figure(figsize=plt.figaspect(0.5) * 1.5) ax = fig.gca(projection='3d') line_styles = ["b", "r", "c", "g"] ss = [0.05, 0.1, 0.15, 0.2, 0.25, 0.3] ax.scatter(c0[0], c0[1], c0[2], c='k', marker='o') for i in range(p.shape[0]): ax.scatter(p[i, 0], p[i, 1], p[i, 2], c=line_styles[i % len(line_styles)], marker='o') for s in ss: ax.scatter(p[i, 0] + s * N[i, 0], p[i, 1] + s * N[i, 1], p[i, 2] + s * N[i, 2], c=line_styles[i % len(line_styles)], marker='x') for s in ss: ax.scatter(c0[0] + s * dc0[0], c0[1] + s * dc0[1], c0[2] + s * dc0[2], c='k', marker='x') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') comAccLP = ComAccLP("comAccLP", c0, dc0, p, N, mu, g_vector, mass, verb=1, regularization=1e-10) comAccLP2 = ComAccLP("comAccLP2", c0, dc0, p, N, mu, g_vector, mass, verb=1, regularization=1e-5) comAccPP = ComAccPP("comAccPP", c0, dc0, p, N, mu, g_vector, mass, verb=0) alpha = 0.0 ddAlpha = -1.0 while (ddAlpha < 0.0): (imode, ddAlpha, slope, alpha_min, alpha_max) = comAccLP.compute_max_deceleration(alpha) (imode3, ddAlpha3, slope3, alpha_min3, alpha_max3) = comAccLP2.compute_max_deceleration(alpha) if (imode == 0): # print "LP ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f, qp time %.3f)"%(alpha,ddAlpha,slope,alpha_max,1e3*comAccLP.qpTime); f = comAccLP.getContactForces() # print "Contact forces:", norm(f, axis=0) else: print "LP failed!" if (imode3 == 0): # print "LP2 ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f, qp time %.3f)"%(alpha,ddAlpha3,slope3,alpha_max3,1e3*comAccLP2.qpTime); f = comAccLP2.getContactForces() # print "Contact forces:", norm(f, axis=0) else: print "LP failed!" (imode2, ddAlpha2, slope2, alpha_max2) = comAccPP.compute_max_deceleration(alpha) if (imode2 == 0): pass # print "PP ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f)"%(alpha,ddAlpha2,slope2,alpha_max2); else: print "PP failed!" if (imode != 0 or imode2 != 0): break if (abs(ddAlpha - ddAlpha2) > 1e-4): print "\n *** ERROR ddAlpha", ddAlpha - ddAlpha2 if (abs(slope - slope2) > 1e-3): print "\n *** ERROR slope", slope - slope2 if (abs(alpha_max - alpha_max2) > 1e-3): if (alpha_max < alpha_max2): print "\n *** WARNING alpha_max underestimated", alpha_max - alpha_max2 else: print "\n *** ERROR alpha_max", alpha_max - alpha_max2 if (abs(ddAlpha3 - ddAlpha2) > 1e-4): print "\n *** ERROR2 ddAlpha", ddAlpha3 - ddAlpha2 if (abs(slope3 - slope2) > 1e-3): print "\n *** ERROR2 slope", slope3 - slope2 if (abs(alpha_max3 - alpha_max2) > 1e-3): if (alpha_max3 < alpha_max2): print "\n *** WARNING2 alpha_max underestimated", alpha_max3 - alpha_max2 else: print "\n *** ERROR2 alpha_max", alpha_max3 - alpha_max2 alpha = alpha_max + EPS # print "\n\n\n SECOND TEST \n\n\n" # from com_acc_LP_backup import ComAccLP as ComAccLP_LP # alpha = 0.0; # ddAlpha = -1.0; # while(ddAlpha<0.0): # (imode, ddAlpha, slope, alpha_min, alpha_max) = comAccLP2.compute_max_deceleration(alpha); # if(imode==0): # print "ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f, qp time %.3f)"%(alpha,ddAlpha,slope,alpha_max,1e3*comAccLP2.qpTime); # else: # print "LP failed!" # # (imode2, ddAlpha2, slope2, alpha_max2) = comAccPP.compute_max_deceleration(alpha); # if(imode2==0): # print "ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f)"%(alpha,ddAlpha2,slope2,alpha_max2); # else: # print "PP failed!"; # # if(imode!= 0 or imode2!=0): # break; # if(abs(ddAlpha-ddAlpha2) > 1e-4): # print "\n *** ERROR ddAlpha", ddAlpha-ddAlpha2; # if(abs(slope-slope2) > 1e-3): # print "\n *** ERROR slope", slope-slope2; # if(abs(alpha_max-alpha_max2) > 1e-3): # if(alpha_max < alpha_max2): # print "\n *** WARNING alpha_max", alpha_max-alpha_max2; # else: # print "\n *** ERROR alpha_max", alpha_max-alpha_max2; # alpha = alpha_max+EPS; if (DO_PLOTS): comAccPP.plotFeasibleAccPolytope()
def test(): DO_PLOTS = False PLOT_3D = False mass = 75 # mass of the robot mu = 0.5 # friction coefficient lx = 0.1 # half foot size in x direction ly = 0.07 # half foot size in y direction USE_DIAGONAL_GENERATORS = True GENERATE_QUASI_FLAT_CONTACTS = True #First, generate a contact configuration CONTACT_POINT_UPPER_BOUNDS = [0.5, 0.5, 0.0] CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0] gamma = atan(mu) # half friction cone angle RPY_LOWER_BOUNDS = [-0 * gamma, -0 * gamma, -pi] RPY_UPPER_BOUNDS = [+0 * gamma, +0 * gamma, +pi] MIN_CONTACT_DISTANCE = 0.3 N_CONTACTS = 2 READ_CONTACTS_FROM_FILE = True X_MARG = 0.07 Y_MARG = 0.07 if (READ_CONTACTS_FROM_FILE): import pickle f = open("./data.pkl", 'rb') res = pickle.load(f) f.close() # (p, N) = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, GENERATE_QUASI_FLAT_CONTACTS); p = res['contact_points'].T N = res['contact_normals'].T print "Contact points\n", p print "Contact normals\n", 1e3 * N X_LB = np.min(p[:, 0] - X_MARG) X_UB = np.max(p[:, 0] + X_MARG) Y_LB = np.min(p[:, 1] - Y_MARG) Y_UB = np.max(p[:, 1] + Y_MARG) Z_LB = np.min(p[:, 2] - 0.05) Z_UB = np.max(p[:, 2] + 1.5) (H, h) = compute_GIWC(p, N, mu, False, USE_DIAGONAL_GENERATORS) (succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h) if (not succeeded): print "Impossible to find a static equilibrium CoM position with the contacts read from file" return else: succeeded = False while (succeeded == False): (p, N) = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, GENERATE_QUASI_FLAT_CONTACTS) X_LB = np.min(p[:, 0] - X_MARG) X_UB = np.max(p[:, 0] + X_MARG) Y_LB = np.min(p[:, 1] - Y_MARG) Y_UB = np.max(p[:, 1] + Y_MARG) Z_LB = np.min(p[:, 2] - 0.05) Z_UB = np.max(p[:, 2] + 1.5) (H, h) = compute_GIWC(p, N, mu, False, USE_DIAGONAL_GENERATORS) (succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h) dc0 = np.random.uniform(-1, 1, size=3) dc0[2] = 0 if (DO_PLOTS): f, ax = plut.create_empty_figure() for j in range(p.shape[0]): ax.scatter(p[j, 0], p[j, 1], c='k', s=100) ax.scatter(c0[0], c0[1], c='r', s=100) com_x = np.zeros(2) com_y = np.zeros(2) com_x[0] = c0[0] com_y[0] = c0[1] com_x[1] = c0[0] + dc0[0] com_y[1] = c0[1] + dc0[1] ax.plot(com_x, com_y, color='b') plt.axis([X_LB, X_UB, Y_LB, Y_UB]) plt.title('Contact Points and CoM position') if (PLOT_3D): fig = plt.figure(figsize=plt.figaspect(0.5) * 1.5) ax = fig.gca(projection='3d') line_styles = ["b", "r", "c", "g"] ss = [0.05, 0.1, 0.15, 0.2, 0.25, 0.3] ax.scatter(c0[0], c0[1], c0[2], c='k', marker='o') for i in range(p.shape[0]): ax.scatter(p[i, 0], p[i, 1], p[i, 2], c=line_styles[i % len(line_styles)], marker='o') for s in ss: ax.scatter(p[i, 0] + s * N[i, 0], p[i, 1] + s * N[i, 1], p[i, 2] + s * N[i, 2], c=line_styles[i % len(line_styles)], marker='x') for s in ss: ax.scatter(c0[0] + s * dc0[0], c0[1] + s * dc0[1], c0[2] + s * dc0[2], c='k', marker='x') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') # return can_I_stop(c0, dc0, p, N, mu, mass, 1.0, 100, DO_PLOTS=DO_PLOTS); (has_stopped, c_final, dc_final) = can_I_stop(c0, dc0, p, N, mu, mass, 1.0, 100, DO_PLOTS=DO_PLOTS) print "Contact points\n", p print "Contact normals\n", N print "Initial com position", c0 print "Initial com velocity", dc0, "norm %.3f" % norm(dc0) print "Final com position", c_final print "Final com velocity", dc_final, "norm %.3f" % norm(dc_final) if (has_stopped): print "The system is stable" else: print "The system is unstable" return True
def test(N_CONTACTS=2, verb=0, solver='qpoases'): from math import atan, pi from pinocchio_inv_dyn.multi_contact.utils import generate_contacts, find_static_equilibrium_com, compute_GIWC, compute_support_polygon import pinocchio_inv_dyn.plot_utils as plut from pinocchio_inv_dyn.geom_utils import plot_inequalities import matplotlib.pyplot as plt np.set_printoptions(precision=2, suppress=True, linewidth=250) DO_PLOTS = True PLOT_3D = False mass = 75 # mass of the robot g_vector = np.array([0, 0, -9.81]) mu = 0.5 # friction coefficient lx = 0.1 # half foot size in x direction ly = 0.07 # half foot size in y direction USE_DIAGONAL_GENERATORS = True GENERATE_QUASI_FLAT_CONTACTS = False #First, generate a contact configuration CONTACT_POINT_UPPER_BOUNDS = [0.5, 0.5, 0.5] CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0] gamma = atan(mu) # half friction cone angle RPY_LOWER_BOUNDS = [-2 * gamma, -2 * gamma, -pi] RPY_UPPER_BOUNDS = [+2 * gamma, +2 * gamma, +pi] MIN_CONTACT_DISTANCE = 0.3 X_MARG = 0.07 Y_MARG = 0.07 succeeded = False while (succeeded == False): (p, N) = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, GENERATE_QUASI_FLAT_CONTACTS) X_LB = np.min(p[:, 0] - X_MARG) X_UB = np.max(p[:, 0] + X_MARG) Y_LB = np.min(p[:, 1] - Y_MARG) Y_UB = np.max(p[:, 1] + Y_MARG) Z_LB = np.min(p[:, 2] - 0.05) Z_UB = np.max(p[:, 2] + 1.5) (H, h) = compute_GIWC(p, N, mu, False, USE_DIAGONAL_GENERATORS) (succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h) dc0 = np.random.uniform(-1, 1, size=3) dc0[2] = 0 if (PLOT_3D): fig = plt.figure(figsize=plt.figaspect(0.5) * 1.5) ax = fig.gca(projection='3d') line_styles = ["b", "r", "c", "g"] ss = [0.05, 0.1, 0.15, 0.2, 0.25, 0.3] ax.scatter(c0[0], c0[1], c0[2], c='k', marker='o') for i in range(p.shape[0]): ax.scatter(p[i, 0], p[i, 1], p[i, 2], c=line_styles[i % len(line_styles)], marker='o') for s in ss: ax.scatter(p[i, 0] + s * N[i, 0], p[i, 1] + s * N[i, 1], p[i, 2] + s * N[i, 2], c=line_styles[i % len(line_styles)], marker='x') for s in ss: ax.scatter(c0[0] + s * dc0[0], c0[1] + s * dc0[1], c0[2] + s * dc0[2], c='k', marker='x') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') a = dc0 / np.linalg.norm(dc0) equiLP = EquilibriumExtremumLP("quiLP", p, N, mu, g_vector, mass, a, c0, verb=verb, solver=solver) (status, com_extr) = equiLP.find_extreme_com_position() print "status", optim.LP_status_string[status], "com", com_extr if (DO_PLOTS): f, ax = plut.create_empty_figure() for j in range(p.shape[0]): ax.scatter(p[j, 0], p[j, 1], c='k', s=100) ax.scatter(c0[0], c0[1], c='r', s=100) com_x = np.zeros(2) com_y = np.zeros(2) com_x[0] = c0[0] com_y[0] = c0[1] com_x[1] = c0[0] + 0.1 * dc0[0] com_y[1] = c0[1] + 0.1 * dc0[1] ax.plot(com_x, com_y, color='b') ax.scatter(com_extr[0], com_extr[1], c='r', s=100) plt.axis([X_LB, X_UB, Y_LB, Y_UB]) plt.title('Contact Points and CoM position') (H, h) = compute_GIWC(p, N, mu) (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.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]) Y_MIN -= 0.1 * (Y_MAX - Y_MIN) Y_MAX += 0.1 * (Y_MAX - Y_MIN) plot_inequalities(B_sp, b_sp, [X_MIN, X_MAX], [Y_MIN, Y_MAX], ax=ax, color='b', lw=4) plt.show()
x_com = createListOfMatrices(N_SOLVERS, (3, conf.MAX_TEST_DURATION)) dx_com = createListOfMatrices(N_SOLVERS, (3, conf.MAX_TEST_DURATION)) ddx_com = createListOfMatrices(N_SOLVERS, (3, conf.MAX_TEST_DURATION)) n_active_ineq = np.zeros((conf.MAX_TEST_DURATION, N_SOLVERS), dtype=np.int) n_violated_ineq = np.zeros((conf.MAX_TEST_DURATION, N_SOLVERS), dtype=np.int) no_sol_count = np.zeros(N_SOLVERS, dtype=np.int) solver_imode = np.zeros((conf.MAX_TEST_DURATION, N_SOLVERS), dtype=np.int) final_time = np.zeros(N_SOLVERS) final_time_step = np.zeros(N_SOLVERS, np.int) controller_balance = N_SOLVERS * [ False, ] for s in conf.SOLVER_TO_INTEGRATE: controller_balance[s] = startSimulation(q0, v0, s) # cProfile.run('startSimulation(q0, v0, s);'); if (conf.PLAY_MOTION_AT_THE_END): print "Gonna play computed motion" sleep(1) simulator.viewer.play(q[s], dt, 1.0) print "Computed motion finished" if (conf.SHOW_FIGURES and conf.PLOT_COM_TRAJ): f, ax = plot_utils.create_empty_figure(3, 1) ax[0].plot(x_com[s][0, :].A.squeeze(), 'k:') ax[1].plot(x_com[s][1, :].A.squeeze(), 'k:') ax[2].plot(x_com[s][2, :].A.squeeze(), 'k:') ax[0].set_title("Com trajectory") plt.show()
def test(N_CONTACTS = 2, verb=0, solver='qpoases'): from math import atan, pi np.set_printoptions(precision=2, suppress=True, linewidth=250); DO_PLOTS = False; PLOT_3D = False; mass = 75; # mass of the robot g_vector = np.array([0,0,-9.81]); mu = 0.5; # friction coefficient lx = 0.1; # half foot size in x direction ly = 0.07; # half foot size in y direction USE_DIAGONAL_GENERATORS = True; GENERATE_QUASI_FLAT_CONTACTS = False; #First, generate a contact configuration CONTACT_POINT_UPPER_BOUNDS = [ 0.5, 0.5, 0.5]; CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0]; gamma = atan(mu); # half friction cone angle RPY_LOWER_BOUNDS = [-2*gamma, -2*gamma, -pi]; RPY_UPPER_BOUNDS = [+2*gamma, +2*gamma, +pi]; MIN_CONTACT_DISTANCE = 0.3; X_MARG = 0.07; Y_MARG = 0.07; succeeded = False; while(succeeded == False): (p, N) = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, GENERATE_QUASI_FLAT_CONTACTS); X_LB = np.min(p[:,0]-X_MARG); X_UB = np.max(p[:,0]+X_MARG); Y_LB = np.min(p[:,1]-Y_MARG); Y_UB = np.max(p[:,1]+Y_MARG); Z_LB = np.min(p[:,2]-0.05); Z_UB = np.max(p[:,2]+1.5); (H,h) = compute_GIWC(p, N, mu, False, USE_DIAGONAL_GENERATORS); (succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h); dc0 = np.random.uniform(-1, 1, size=3); dc0[2] = 0; if(False and DO_PLOTS): f, ax = plut.create_empty_figure(); for j in range(p.shape[0]): ax.scatter(p[j,0], p[j,1], c='k', s=100); ax.scatter(c0[0], c0[1], c='r', s=100); com_x = np.zeros(2); com_y = np.zeros(2); com_x[0] = c0[0]; com_y[0] = c0[1]; com_x[1] = c0[0]+dc0[0]; com_y[1] = c0[1]+dc0[1]; ax.plot(com_x, com_y, color='b'); plt.axis([X_LB,X_UB,Y_LB,Y_UB]); plt.title('Contact Points and CoM position'); if(PLOT_3D): fig = plt.figure(figsize=plt.figaspect(0.5)*1.5) ax = fig.gca(projection='3d') line_styles =["b", "r", "c", "g"]; ss = [0.05, 0.1, 0.15, 0.2, 0.25, 0.3]; ax.scatter(c0[0],c0[1],c0[2], c='k', marker='o'); for i in range(p.shape[0]): ax.scatter(p[i,0],p[i,1],p[i,2], c=line_styles[i%len(line_styles)], marker='o'); for s in ss: ax.scatter(p[i,0]+s*N[i,0],p[i,1]+s*N[i,1],p[i,2]+s*N[i,2], c=line_styles[i%len(line_styles)], marker='x'); for s in ss: ax.scatter(c0[0]+s*dc0[0],c0[1]+s*dc0[1],c0[2]+s*dc0[2], c='k', marker='x'); ax.set_xlabel('x'); ax.set_ylabel('y'); ax.set_zlabel('z'); comAccLP = ComAccLP("comAccLP", c0, dc0, p, N, mu, g_vector, mass, verb=verb, regularization=1e-5, solver=solver); # comAccLP2 = ComAccLP("comAccLP2", c0, dc0, p, N, mu, g_vector, mass, verb=verb, regularization=1e-5); comAccPP = ComAccPP("comAccPP", c0, dc0, p, N, mu, g_vector, mass, verb=verb); alpha = 0.0; ddAlpha = -1.0; while(ddAlpha<0.0): if(verb>1): print "Compute comAccLP with alpha=%.3f"%alpha; (imode, ddAlpha, slope, alpha_min, alpha_max) = comAccLP.compute_max_deceleration(alpha); # (imode3, ddAlpha3, slope3, alpha_min3, alpha_max3) = comAccLP2.compute_max_deceleration(alpha); # if(imode3==0): # print "LP2 ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f, qp time %.3f)"%(alpha,ddAlpha3,slope3,alpha_max3,1e3*comAccLP2.qpTime); # f = comAccLP2.getContactForces(); # print "Contact forces:", norm(f, axis=0) # else: # print "LP failed!" if(verb>1): print "Compute comAccPP with alpha=%.3f"%alpha; (imode2, ddAlpha2, slope2, alpha_max2) = comAccPP.compute_max_deceleration(alpha); if(imode!=0 and imode2==0): print "ERROR LP failed while PP did not!"; if(imode==0 and imode2!=0): print "ERROR PP failed while LP did not!"; error = False; if(imode!= 0 or imode2!=0): break; if(abs(ddAlpha-ddAlpha2) > 1e-4): print "\n *** ERROR ddAlpha", ddAlpha-ddAlpha2; error = True; if(abs(slope-slope2) > 1e-3): print "\n *** ERROR slope", slope-slope2; error = True; if(abs(alpha_max-alpha_max2) > 1e-3): if(alpha_max < alpha_max2): print "\n *** WARNING alpha_max underestimated", alpha_max-alpha_max2; else: print "\n *** ERROR alpha_max", alpha_max-alpha_max2; error = True; if(error and verb>0): print "LP ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f, qp time %.3f)"%(alpha,ddAlpha,slope,alpha_max,1e3*comAccLP.getLpTime()); f = comAccLP.getContactForces(); #print "Contact forces:\n", f; print "Contact force norms:", norm(f, axis=0); print "Contact force generator coefficients:", comAccLP.x; #print "Contact force generators:\n", comAccLP.G4[:,:16]; print "PP ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f)"%(alpha,ddAlpha2,slope2,alpha_max2); print "p:", p.T; print "N", N.T; print "c", c0.T; print "dc", dc0.T; alpha = alpha_max+EPS; # print "\n\n\n SECOND TEST \n\n\n" # from com_acc_LP_backup import ComAccLP as ComAccLP_LP # alpha = 0.0; # ddAlpha = -1.0; # while(ddAlpha<0.0): # (imode, ddAlpha, slope, alpha_min, alpha_max) = comAccLP2.compute_max_deceleration(alpha); # if(imode==0): # print "ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f, qp time %.3f)"%(alpha,ddAlpha,slope,alpha_max,1e3*comAccLP2.qpTime); # else: # print "LP failed!" # # (imode2, ddAlpha2, slope2, alpha_max2) = comAccPP.compute_max_deceleration(alpha); # if(imode2==0): # print "ddAlpha_min(%.2f) = %.3f (slope %.3f, alpha_max %.3f)"%(alpha,ddAlpha2,slope2,alpha_max2); # else: # print "PP failed!"; # # if(imode!= 0 or imode2!=0): # break; # if(abs(ddAlpha-ddAlpha2) > 1e-4): # print "\n *** ERROR ddAlpha", ddAlpha-ddAlpha2; # if(abs(slope-slope2) > 1e-3): # print "\n *** ERROR slope", slope-slope2; # if(abs(alpha_max-alpha_max2) > 1e-3): # if(alpha_max < alpha_max2): # print "\n *** WARNING alpha_max", alpha_max-alpha_max2; # else: # print "\n *** ERROR alpha_max", alpha_max-alpha_max2; # alpha = alpha_max+EPS; if(DO_PLOTS): comAccPP.plotFeasibleAccPolytope();
ee_traj[i] = SmoothedSE3Trajectory("ee_traj_"+ee_names[i], ee_ref[i], dt, conf.SMOOTH_FILTER_WINDOW_LENGTH); ee_tasks[i] = SE3Task(invDynForm.r, ee, ee_traj[i], ee_names[i]); ee_tasks[i].kp = conf.kp_ee; ee_tasks[i].kv = conf.kd_ee; ee_tasks[i].mask(conf.ee_mask); ''' CREATE COM TASK ''' com_traj = SmoothedNdTrajectory("com_traj", com_ref, conf.dt, conf.SMOOTH_FILTER_WINDOW_LENGTH); com_task = CoMTask(invDynForm.r, com_traj); com_task.kp = conf.kp_com; com_task.kv = conf.kd_com; invDynForm.addTask(com_task, conf.w_com); if(conf.SHOW_FIGURES and conf.PLOT_REF_EE_TRAJ): for (i,ee) in enumerate(ee_indexes): f, ax = plot_utils.create_empty_figure(3, 3); for j in range(3): ax[j,0].plot(ee_traj[i]._x_ref[j, :].A.squeeze()); ax[j,0].plot(np.hstack([M.translation for M in ee_ref[i]])[j,:].A.squeeze(), 'r--'); ax[j,1].plot(ee_traj[i]._v_ref[j, :].A.squeeze()); ax[j,2].plot(ee_traj[i]._a_ref[j, :].A.squeeze()); ax[0,0].set_title("Pos "+ee_names[i].replace('_','\_')); ax[0,1].set_title("Vel"); ax[0,2].set_title("Acc"); plt.show(); if(conf.SHOW_FIGURES and conf.PLOT_REF_COM_TRAJ): for j in range(3): f, ax = plot_utils.create_empty_figure(3, 1); ax[0].plot(com_traj._x_ref[j, :].A.squeeze()); ax[1].plot(com_traj._v_ref[j, :].A.squeeze());