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(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_continuous_cpp_vs_continuous_py(N_CONTACTS = 2, solver='qpoases', verb=0):
DO_PLOTS = False;
PLOT_3D = False;
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;
global mass
global g_vector
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.max(p[:,2]+0.3);
Z_UB = np.max(p[:,2]+1.5);
#~ (H,h) = compute_GIWC(p, N, mu, False);
H = -compute_CWC(p, N, mass, mu); h = zeros(H.shape[0])
(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);
Z_MIN = np.max(p[:,2])-0.1;
Ineq_kin = zeros([3,3]); Ineq_kin[2,2] = -1
ineq_kin = zeros(3); ineq_kin[2] = -Z_MIN
bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver, kinematic_constraints = [Ineq_kin,ineq_kin ]);
eqCpp = Equilibrium("dyn_eq2", mass, 4)
eqCpp.setNewContacts(asmatrix(p),asmatrix(N),mu,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
#~ bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver, kinematic_constraints = None);
#~ stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver);
window_times = [1]+ [0.1*i for i in range(1,10)] + [0.1*i for i in range(11,21)] #try nominal time first
#~ window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)] #try nominal time first
#~ window_times = [1]+ [0.4*i for i in range(1,4)] #try nominal time first
#~ window_times = [1]+ [0.4*i for i in range(3,6)] #try nominal time first
#~ window_times = [0.7]
found = False
time_step_check = -0.2
for i, el in enumerate(window_times):
if (found):
break
res = bezierSolver.can_I_stop(T=el, time_step = time_step_check);
if (res.is_stable):
found = True
#~ print "continuous found at ", el
__check_trajectory(bezierSolver._p0, bezierSolver._p1, res.c, res.c, el, bezierSolver._H,
bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = bezier(matrix([p_i.tolist() for p_i in res.wpsdL]).transpose()))
if i != 0:
print "continuous Failed to stop at 1, but managed to stop at ", el
found = False
time_step_check = 0.05
for i, el in enumerate(window_times):
if (found):
break
res2 = bezierSolver.can_I_stop(T=el, time_step = time_step_check, l0 = zeros(3));
if (res2.is_stable):
found = True
#~ print "ang_momentum found at ", el
__check_trajectory(bezierSolver._p0, bezierSolver._p1, res2.c, res2.c, el, bezierSolver._H,
#~ bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = res2.dL_of_t)
bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = bezier(matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose()))
if i != 0:
print "ang_momentum Failed to stop at 1, but managed to stop at ", el
#~ res2 = None
#~ try:
#~ res2 = stabilitySolver.can_I_stop();
#~ except Exception as e:
#~ pass
if(res2.is_stable != res.is_stable ):
if(res.is_stable):
print "continuous won"
else:
print "ang_momentum won"
return res2.is_stable, res.is_stable, res2, res, c0, dc0, H, h, p, N
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()
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();
def test_continuous_vs_discretize(N_CONTACTS=2, solver='qpoases', verb=0):
DO_PLOTS = False
PLOT_3D = False
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
global mass
global g_vector
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.max(p[:, 2] + 0.3)
Z_UB = np.max(p[:, 2] + 1.5)
#~ (H,h) = compute_GIWC(p, N, mu, False);
H = -compute_CWC(p, N, mass, mu)
h = zeros(H.shape[0])
(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)
Z_MIN = np.max(p[:, 2]) - 0.1
Ineq_kin = zeros([3, 3])
Ineq_kin[2, 2] = -1
ineq_kin = zeros(3)
ineq_kin[2] = -Z_MIN
bezierSolver = BezierZeroStepCapturability(
"ss",
c0,
dc0,
p,
N,
mu,
g_vector,
mass,
verb=verb,
solver=solver,
kinematic_constraints=[Ineq_kin, ineq_kin])
#~ bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver, kinematic_constraints = None);
stabilitySolver = StabilityCriterion("ss",
c0,
dc0,
p,
N,
mu,
g_vector,
mass,
verb=verb,
solver=solver)
window_times = [1] + [0.1 * i for i in range(1, 10)
] + [0.1 * i
for i in range(11, 20)] #try nominal time first
#~ window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)] #try nominal time first
#~ window_times = [1]+ [0.4*i for i in range(1,4)] #try nominal time first
#~ window_times = [1]+ [0.4*i for i in range(3,6)] #try nominal time first
window_times = [0.7]
found = False
time_step_check = 0.05
for i, el in enumerate(window_times):
if (found):
break
res = bezierSolver.can_I_stop(T=el, time_step=time_step_check)
if (res.is_stable):
found = True
#~ print "found at ", el
__check_trajectory(bezierSolver._p0,
bezierSolver._p1,
res.c,
res.c,
el,
bezierSolver._H,
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check)
if i != 0:
print "discretize Failed to stop at 1, but managed to stop at ", el
found = False
time_step_check = -0.2
for i, el in enumerate(window_times):
if (found):
break
res2 = bezierSolver.can_I_stop(T=el, time_step=time_step_check)
if (res2.is_stable):
found = True
#~ print "found at ", el
__check_trajectory(bezierSolver._p0,
bezierSolver._p1,
res2.c,
res2.c,
el,
bezierSolver._H,
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check)
if i != 0:
print "continuous Failed to stop at 1, but managed to stop at ", el
if (res2.is_stable != res.is_stable):
if (res.is_stable):
print "discretize won"
else:
print "continuous won"
return res.is_stable, res2.is_stable, res, res2, c0, dc0, H, h, p, N
