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_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 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)