def plotFeasibleAccPolytope(self, range_plot=2.0):
from pinocchio_inv_dyn.geom_utils import plot_inequalities
ax = plot_inequalities(np.vstack([self.a,self.b]).T, self.d, [-range_plot,range_plot], [-range_plot,range_plot]);
plt.axis([0,range_plot,-range_plot,0])
plt.title('Feasible com pos-acc');
plut.movePlotSpines(ax, [0, 0]);
ax.set_xlabel('com pos');
ax.set_ylabel('com acc');
plt.show();
def can_I_stop(c0,
dc0,
contact_points,
contact_normals,
mu,
mass,
T_0,
MAX_ITER=1000,
DO_PLOTS=False,
verb=0,
eliminate_redundancies=False):
''' Determine whether the system can come to a stop without changing contacts.
Keyword arguments:
c0 -- initial CoM position
dc0 -- initial CoM velocity
contact points -- a matrix containing the contact points
contact normals -- a matrix containing the contact normals
mu -- friction coefficient (either a scalar or an array)
mass -- the robot mass
T_0 -- an initial guess for the time to stop
Output: (is_stable, c_final, dc_final), where:
is_stable -- boolean value
c_final -- final com position
dc_final -- final com velocity
'''
# Steps:
# - Compute GIWC: H*w <= h, where w=(m*(g-ddc), m*cx(g-ddc))
# - Project GIWC in (alpha,DDalpha) space, where c=c0+alpha*v, ddc=-DDalpha*v, v=dc0/||dc0||: A*(alpha,DDalpha)<=b
# - Find ordered (left-most first, clockwise) vertices of 2d polytope A*(alpha,DDalpha)<=b: V
# - Initialize: alpha=0, Dalpha=||dc0||
# - LOOP:
# - Find current active inequality: a*alpha + b*DDalpha <= d (i.e. DDalpha upper bound for current alpha value)
# - If DDalpha_upper_bound<0: return False
# - Find alpha_max (i.e. value of alpha corresponding to right vertex of active inequality)
# - Initialize: t=T_0, t_ub=10, t_lb=0
# LOOP:
# - Integrate LDS until t: DDalpha = d/b - (a/d)*alpha
# - if(Dalpha(t)==0 && alpha(t)<=alpha_max): return (True, alpha(t)*v)
# - if(alpha(t)==alpha_max && Dalpha(t)>0): alpha=alpha(t), Dalpha=Dalpha(t), break
# - if(alpha(t)<alpha_max && Dalpha>0): t_lb=t, t=(t_ub+t_lb)/2
# - else t_ub=t, t=(t_ub+t_lb)/2
assert mass > 0.0, "Mass is not positive"
assert T_0 > 0.0, "Time is not positive"
assert mu > 0.0, "Friction coefficient is not positive"
c0 = np.asarray(c0).squeeze()
dc0 = np.asarray(dc0).squeeze()
contact_points = np.asarray(contact_points)
contact_normals = np.asarray(contact_normals)
assert c0.shape[0] == 3, "Com position has not size 3"
assert dc0.shape[0] == 3, "Com velocity has not size 3"
assert contact_points.shape[1] == 3, "Contact points have not size 3"
assert contact_normals.shape[1] == 3, "Contact normals have not size 3"
assert contact_points.shape[0] == contact_normals.shape[
0], "Number of contact points and contact normals do not match"
g_vector = np.array([0, 0, -9.81])
(H, h) = compute_GIWC(contact_points,
contact_normals,
mu,
eliminate_redundancies=eliminate_redundancies)
# If initial com velocity is zero then test static equilibrium
if (np.linalg.norm(dc0) < EPS):
w = np.zeros(6)
w[2] = -mass * 9.81
w[3:] = mass * np.cross(c0, g_vector)
if (np.max(np.dot(H, w) - h) < EPS):
return (True, c0, dc0)
return (False, c0, dc0)
# Project GIWC in (alpha,DDalpha) space, where c=c0+alpha*v, ddc=DDalpha*v, v=dc0/||dc0||: a*alpha + b*DDalpha <= d
v = dc0 / np.linalg.norm(dc0)
K = np.zeros((6, 3))
K[:3, :] = mass * np.identity(3)
K[3:, :] = mass * crossMatrix(c0)
d = h - np.dot(H, np.dot(K, g_vector))
b = np.dot(np.dot(-H, K), v)
# vector multiplying com acceleration
tmp = np.array([
[0, 0, 0, 0, 1, 0], #temp times the variation dc will result in
[0, 0, 0, -1, 0, 0], # [ 0 0 0 dc_y -dc_x 0]^T
[0, 0, 0, 0, 0, 0]
]).T
a = mass * 9.81 * np.dot(np.dot(H, tmp), v)
if (DO_PLOTS):
range_plot = 10
ax = plot_inequalities(
np.vstack([a, b]).T, d, [-range_plot, range_plot],
[-range_plot, range_plot])
plt.axis([0, range_plot, -range_plot, 0])
plt.title('Feasible com pos-acc')
plut.movePlotSpines(ax, [0, 0])
ax.set_xlabel('com pos')
ax.set_ylabel('com acc')
plt.show()
# Eliminate redundant inequalities
if (eliminate_redundancies):
A_red, d = eliminate_redundant_inequalities(np.vstack([a, b]).T, d)
a = A_red[:, 0]
b = A_red[:, 1]
# Normalize inequalities to have unitary coefficients for DDalpha: b*DDalpha <= d - a*alpha
for i in range(a.shape[0]):
if (abs(b[i]) > EPS):
a[i] /= abs(b[i])
d[i] /= abs(b[i])
b[i] /= abs(b[i])
elif (verb > 0):
print "WARNING: cannot normalize %d-th inequality because coefficient of DDalpha is almost zero" % i, b[
i]
# Initialize: alpha=0, Dalpha=||dc0||
alpha = 0
Dalpha = np.linalg.norm(dc0)
#sort b indices to only keep negative values
negative_ids = np.where(b < 0)[0]
if (negative_ids.shape[0] == 0):
# CoM acceleration is unbounded
return (True, c0, 0.0 * v)
for iiii in range(MAX_ITER):
# Find current active inequality: b*DDalpha <= d - a*alpha (i.e. DDalpha lower bound for current alpha value)
a_alpha_d = a * alpha - d
a_alpha_d_negative_bs = a_alpha_d[negative_ids]
(i_DDalpha_min, DDalpha_min) = [
(i, a_min)
for (i, a_min) in [(j, a_alpha_d[j]) for j in negative_ids]
if (a_min >= a_alpha_d_negative_bs).all()
][0]
if (verb > 0):
print "DDalpha_min", DDalpha_min
print "i_DDalpha_min", i_DDalpha_min
# If DDalpha_lower_bound>0: return False
if (DDalpha_min >= -EPS):
if (verb > 0):
print "Algorithm converged because DDalpha is positive"
return (False, c0 + alpha * v, Dalpha * v)
# Find alpha_max (i.e. value of alpha corresponding to right vertex of active inequality)
den = b * a[i_DDalpha_min] + a
i_pos = np.where(den > 0)[0]
if (i_pos.shape[0] == 0):
# print "WARNING b*a_i0+a is never positive, that means that alpha_max is unbounded";
alpha_max = 10.0
else:
alpha_max = np.min(
(d[i_pos] + b[i_pos] * d[i_DDalpha_min]) / den[i_pos])
if (verb > 0):
print "alpha_max", alpha_max
if (alpha_max < alpha):
# We reach the right limit of the polytope of feasible com pos-acc.
# This means there is no feasible com acc for farther com position (with zero angular momentum derivative)
return (False, c0 + alpha * v, Dalpha * v)
# If DDalpha is not always negative on the current segment then update alpha_max to the point
# where the current segment intersects the x axis
if (a[i_DDalpha_min] * alpha_max - d[i_DDalpha_min] > 0.0):
alpha_max = d[i_DDalpha_min] / a[i_DDalpha_min]
if (verb > 0):
print "Updated alpha_max", alpha_max
# Initialize: t=T_0, t_ub=10, t_lb=0
t = T_0
t_ub = 10
t_lb = 0
if (abs(a[i_DDalpha_min]) > EPS):
omega = sqrt(a[i_DDalpha_min] + 0j)
if (verb > 0):
print "omega", omega
else:
# if the acceleration is constant over time I can compute directly the time needed
# to bring the velocity to zero:
# Dalpha(t) = Dalpha(0) + t*DDalpha = 0
# t = -Dalpha(0)/DDalpha
t = Dalpha / d[i_DDalpha_min]
alpha_t = alpha + t * Dalpha - 0.5 * t * t * d[i_DDalpha_min]
if (alpha_t <= alpha_max + EPS):
if (verb > 0):
print "DDalpha_min is independent from alpha, algorithm converged to Dalpha=0"
return (True, c0 + alpha_t * v, 0.0 * v)
# if alpha reaches alpha_max before the velocity is zero, then compute the time needed to reach alpha_max
# alpha(t) = alpha(0) + t*Dalpha(0) + 0.5*t*t*DDalpha = alpha_max
# t = (- Dalpha(0) +/- sqrt(Dalpha(0)^2 - 2*DDalpha(alpha(0)-alpha_max))) / DDalpha;
# where DDalpha = -d[i_DDalpha_min]
# Having two solutions, we take the smallest one because we want to find the first time
# at which alpha reaches alpha_max
delta = sqrt(Dalpha**2 + 2 * d[i_DDalpha_min] *
(alpha - alpha_max))
t = (Dalpha - delta) / d[i_DDalpha_min]
if (t < 0.0):
# If the smallest time at which alpha reaches alpha_max is negative print a WARNING because this should not happen
print "WARNING: Time is less than zero:", t, alpha, Dalpha, d[
i_DDalpha_min], alpha_max
t = (Dalpha + delta) / d[i_DDalpha_min]
if (t < 0.0):
# If also the largest time is negative print an ERROR and return
print "ERROR: Time is still less than zero:", t, alpha, Dalpha, d[
i_DDalpha_min], alpha_max
return (False, c0 + alpha * v, Dalpha * v)
bisection_converged = False
for jjjj in range(MAX_ITER):
# Integrate LDS until t: DDalpha = a*alpha - d
if (abs(a[i_DDalpha_min]) > EPS):
# if a=0 then the acceleration is a linear function of the position and I need to use this formula to integrate
sh = np.sinh(omega * t)
ch = np.cosh(omega * t)
alpha_t = ch * alpha + sh * Dalpha / omega + (1 - ch) * (
d[i_DDalpha_min] / a[i_DDalpha_min])
Dalpha_t = omega * sh * alpha + ch * Dalpha - omega * sh * (
d[i_DDalpha_min] / a[i_DDalpha_min])
else:
# if a=0 then the acceleration is constant and I need to use this formula to integrate
alpha_t = alpha + t * Dalpha - 0.5 * t * t * d[i_DDalpha_min]
Dalpha_t = Dalpha - t * d[i_DDalpha_min]
if (np.imag(alpha_t) != 0.0):
print "ERROR alpha is imaginary", alpha_t
return (False, c0 + alpha * v, Dalpha * v)
if (np.imag(Dalpha_t) != 0.0):
print "ERROR Dalpha is imaginary", Dalpha_t
return (False, c0 + alpha * v, Dalpha * v)
alpha_t = np.real(alpha_t)
Dalpha_t = np.real(Dalpha_t)
if (verb > 0):
print "Bisection iter", jjjj, "alpha", alpha_t, "Dalpha", Dalpha_t, "t", t
if (abs(Dalpha_t) < EPS and alpha_t <= alpha_max + EPS):
if (verb > 0):
print "Algorithm converged to Dalpha=0"
return (True, c0 + alpha_t * v, Dalpha_t * v)
if (abs(alpha_t - alpha_max) < EPS and Dalpha_t > 0):
alpha = alpha_max + EPS
Dalpha = Dalpha_t
bisection_converged = True
break
if (alpha_t < alpha_max and Dalpha_t > 0):
t_lb = t
t = (t_ub + t_lb) / 2
else:
t_ub = t
t = (t_ub + t_lb) / 2
if (not bisection_converged):
print "ERROR: Bisection search did not converge in %d iterations" % MAX_ITER
return (False, c0 + alpha * v, Dalpha * v)
print "ERROR: Numerical integration did not converge in %d iterations" % MAX_ITER
if (DO_PLOTS):
plt.show()
return (False, c0 + alpha * v, Dalpha * v)
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()