def compute_com_acceleration_polytope(com_pos,
H,
h,
mass,
g_vector,
eliminate_redundancies=False):
''' Compute the inequalities A*x<=b defining the polytope of feasible CoM accelerations
assuming zero rate of change of angular momentum.
@param c0 Current com position
@param H Matrix of GIWC, which can be computed by compute_GIWC
@param h Vector of GIWC, which can be computed by compute_GIWC
@param mass Mass of the system in Kg
@param g_vector Gravity vector
@return (A,b)
'''
K = np.zeros((6, 3))
K[:3, :] = mass * np.identity(3)
K[3:, :] = mass * crossMatrix(com_pos)
b = h - np.dot(H, np.dot(K, g_vector))
#constant term of F
A = np.dot(-H, K)
#matrix multiplying com acceleration
if (eliminate_redundancies):
(A, b) = eliminate_redundant_inequalities(A, b)
return A, b
def compute_com_acceleration_polytope(com_pos,
H,
h,
mass,
g_vector,
eliminate_redundancies=False):
''' Compute the inequalities A*x<=b defining the polytope of feasible CoM accelerations
assuming zero rate of change of angular momentum.
@param c0 Current com position
@param H Matrix of GIWC, which can be computed by compute_GIWC
@param h Vector of GIWC, which can be computed by compute_GIWC
@param mass Mass of the system in Kg
@param g_vector Gravity vector
@return (A,b)
The com wrench to generate a com acceleration ddc is an affine function
of ddc:
w = K*ddc+d
'''
K = np.zeros((6, 3))
K[:3, :] = mass * np.identity(3)
K[3:, :] = mass * crossMatrix(com_pos)
b = h - np.dot(H, np.dot(K, g_vector))
#constant term of F
A = np.dot(-H, K)
#matrix multiplying com acceleration
if (eliminate_redundancies):
(A, b) = eliminate_redundant_inequalities(A, b)
# normalize inequalities
for i in range(A.shape[0]):
norm_Ai = norm(A[i, :])
if (norm_Ai > EPS):
A[i, :] /= norm_Ai
b[i] /= norm_Ai
return A, b
def compute_support_polygon(H,
h,
mass,
g_vector,
eliminate_redundancies=False):
''' Compute the 2d support polygon A*c<=b given the gravito-inertial wrench
cone (GIWC) as H*w <= h.
Project inequalities from 6d to 2d x-y com space.
The com wrench to maintain static equilibrium is an affine function
of the com position c:
w = D*c+d
Substituting this into the GIWC inequalities we get:
H*w <= h
H*D*c + H*d <= h
H*D*c <= h - H*d
'''
D = np.zeros((6, 3))
d = np.zeros(6)
D[3:, :] = -mass * crossMatrix(g_vector)
d[:3] = mass * g_vector
A = np.dot(H, D[:, :2])
b = h - np.dot(H, d)
if (eliminate_redundancies):
(A, b) = eliminate_redundant_inequalities(A, b)
return (A, b)
def compute_centroidal_cone_generators(contact_points, contact_normals, mu):
''' Compute two matrices. The first one contains the 6d generators of the
centroidal cone. The second one contains the 3d generators of the
contact cones (4 generators for each contact point).
'''
assert contact_points.shape[1] == 3, "Wrong size of contact_points"
assert contact_normals.shape[1] == 3, "Wrong size of contact_normals"
assert contact_points.shape[0] == contact_normals.shape[
0], "Size of contact_points and contact_normals do not match"
contact_points = np.asarray(contact_points)
contact_normals = np.asarray(contact_normals)
nContacts = contact_points.shape[0]
cg = 4
# number of generators for each friction cone
nGen = nContacts * cg
# total number of generators
if (isinstance(mu, (list, tuple, np.ndarray))):
mu = np.asarray(mu).squeeze()
else:
mu = mu * np.ones(nContacts)
G = np.zeros((3, nGen))
A = np.zeros((6, nGen))
P = np.zeros((6, 3))
P[:3, :] = -np.identity(3)
muu = mu / sqrt(2.0)
for i in range(nContacts):
# compute tangent directions
assert norm(contact_normals[
i, :]) != 0.0, "Length of contact normals cannot be zero"
contact_normals[i, :] = contact_normals[i, :] / norm(
contact_normals[i, :])
T1 = np.cross(contact_normals[i, :], [0., 1., 0.])
if (norm(T1) < EPS):
T1 = np.cross(contact_normals[i, :], [1., 0., 0.])
T1 = T1 / norm(T1)
T2 = np.cross(contact_normals[i, :], T1)
G[:, cg * i + 0] = muu[i] * T1 + muu[i] * T2 + contact_normals[i, :]
G[:, cg * i + 1] = muu[i] * T1 - muu[i] * T2 + contact_normals[i, :]
G[:, cg * i + 2] = -muu[i] * T1 + muu[i] * T2 + contact_normals[i, :]
G[:, cg * i + 3] = -muu[i] * T1 - muu[i] * T2 + contact_normals[i, :]
# compute matrix mapping contact forces to gravito-inertial wrench
P[3:, :] = -crossMatrix(contact_points[i, :])
# project generators in 6d centroidal space
A[:, cg * i:cg * i + cg] = np.dot(P, G[:, cg * i:cg * i + cg])
# normalize generators
for i in range(nGen):
G[:, i] /= norm(G[:, i])
A[:, i] /= norm(A[:, i])
return (A, G)
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)