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());
