class ComAccLP(object):
"""
LP solver dedicated to finding the maximum center of mass (CoM) deceleration in a
specified direction, subject to the friction cone constraints.
This is possible thanks to the simplifying assumption that the CoM acceleration
is going to be parallel to its velocity. This allows us to represent the 3d
com trajectory by means of a 1d trajectory alpha(t):
c(t) = c0 + alpha(t) v
v = c0 / ||c0||
dc = dAlpha(t) v
ddc(t) = ddAlpha(t) v
The operation amounts to solving the following parametric Linear Program:
minimize ddAlpha + w sum_i(f_i)
subject to A f = a ddAlpha + b alpha + d
f >= 0
where:
f are the contact forces generator coefficients
ddAlpha is the magnitude of the CoM acceleration
alpha is the magnitude of the CoM displacement (with respect to its initial position c0)
w regularization parameter
Given a CoM position (by means of a value of alpha), this class can compute the
minimum com acceleration in direction v (i.e. the maximum acceleration in direction -v).
Since this is a piecewise-linear function of alpha, it can also compute its derivative with respect
to alpha, and the boundaries of the alpha-region in which the derivative remains constant.
"""
NO_WARM_START = False
name = ""
# solver name
n = 0
# number of variables
m_in = 0
# number of constraints (i.e. 6)
Hess = []
# Hessian
grad = []
# gradient
A = None
# constraint matrix multiplying the contact force generators
b = None
# constraint vector multiplying the CoM position parameter alpha
d = None
# constraint vector
mass = 0.0
# robot mass
g = None
# 3d gravity vector
maxIter = 0
# max number of iterations
verb = 0
# verbosity level of the solver (0=min, 2=max)
iter = 0
# current iteration number
computationTime = 0.0
# total computation time
qpTime = 0.0
# time taken to solve the QP(s) only
initialized = False
# true if solver has been initialized
qpOasesSolver = []
options = []
# qp oases solver's options
epsilon = np.sqrt(np.finfo(float).eps)
INEQ_VIOLATION_THR = 1e-4
def __init__(self,
name,
c0,
v,
contact_points,
contact_normals,
mu,
g,
mass,
maxIter=10000,
verb=0,
regularization=1e-5):
''' Constructor
@param c0 Initial CoM position
@param v Opposite of the direction in which you want to maximize the CoM acceleration (typically that would be
the CoM velocity direction)
@param g Gravity vector
@param regularization Weight of the force minimization, the higher this value, the sparser the solution
'''
self.name = name
self.maxIter = maxIter
self.verb = verb
self.m_in = 6
self.initialized = False
self.options = Options()
self.options.setToReliable()
if (self.verb <= 1):
self.options.printLevel = PrintLevel.NONE
elif (self.verb == 2):
self.options.printLevel = PrintLevel.LOW
elif (self.verb == 3):
self.options.printLevel = PrintLevel.MEDIUM
elif (self.verb > 3):
self.options.printLevel = PrintLevel.DEBUG_ITER
self.options.enableRegularisation = False
self.options.enableEqualities = True
# self.qpOasesSolver.printOptions()
self.b = np.zeros(6)
self.d = np.empty(6)
self.c0 = np.empty(3)
self.v = np.empty(3)
self.constrUB = np.zeros(self.m_in) + 1e100
self.constrLB = np.zeros(self.m_in) - 1e100
self.set_problem_data(c0, v, contact_points, contact_normals, mu, g,
mass, regularization)
def set_com_state(self, c0, v):
assert np.asarray(
c0).squeeze().shape[0] == 3, "Com position vector has not size 3"
assert np.asarray(v).squeeze(
).shape[0] == 3, "Com acceleration direction vector has not size 3"
self.c0 = np.asarray(c0).squeeze()
self.v = np.asarray(v).squeeze().copy()
if (norm(v) == 0.0):
raise ValueError(
"[%s] Norm of com acceleration direction v is zero!" %
self.name)
self.v /= norm(self.v)
self.constrMat[:3, -1] = self.mass * self.v
self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v)
self.b[3:] = self.mass * np.cross(v, self.g)
self.d[:3] = self.mass * self.g
self.d[3:] = self.mass * np.cross(c0, self.g)
def set_contacts(self,
contact_points,
contact_normals,
mu,
regularization=1e-5):
# compute matrix A, which maps the force generator coefficients into the centroidal wrench
(self.A,
self.G4) = compute_centroidal_cone_generators(contact_points,
contact_normals, mu)
# since the size of the problem may have changed we need to recreate the solver and all the problem matrices/vectors
self.n = contact_points.shape[0] * 4 + 1
self.qpOasesSolver = SQProblem(self.n, self.m_in)
#, HessianType.SEMIDEF);
self.qpOasesSolver.setOptions(self.options)
self.Hess = INITIAL_HESSIAN_REGULARIZATION * np.identity(self.n)
self.grad = np.ones(self.n) * regularization
self.grad[-1] = 1.0
self.constrMat = np.zeros((self.m_in, self.n))
self.constrMat[:, :-1] = self.A
self.constrMat[:3, -1] = self.mass * self.v
self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v)
self.lb = np.zeros(self.n)
self.lb[-1] = -1e100
self.ub = np.array(self.n * [
1e100,
])
self.x = np.zeros(self.n)
self.y = np.zeros(self.n + self.m_in)
self.initialized = False
def set_problem_data(self,
c0,
v,
contact_points,
contact_normals,
mu,
g,
mass,
regularization=1e-5):
assert g.shape[0] == 3, "Gravity vector has not size 3"
assert mass > 0.0, "Mass is not positive"
self.mass = mass
self.g = np.asarray(g).squeeze()
self.set_contacts(contact_points, contact_normals, mu, regularization)
self.set_com_state(c0, v)
def compute_max_deceleration_derivative(self):
''' Compute the derivative of the max CoM deceleration (i.e. the solution of the last LP)
with respect to the parameter alpha (i.e. the CoM position parameter). Moreover,
it also computes the bounds within which this derivative is valid (alpha_min, alpha_max).
'''
act_set = np.where(self.y[:self.n - 1] != 0.0)[0]
# indexes of active bound constraints
n_as = act_set.shape[0]
if (n_as > self.n - 6):
raise ValueError(
"[%s] ERROR Too many active constraints: %d (rather than %d)" %
(self.name, n_as, self.n - 6))
if (self.verb > 0 and n_as < self.n - 6):
print "[%s] INFO Less active constraints than expected: %d (rather than %d)" % (
self.name, n_as, self.n - 6)
self.K = np.zeros((n_as + 6, self.n))
self.k1 = np.zeros(n_as + 6)
self.k2 = np.zeros(n_as + 6)
self.K[:n_as, :] = np.identity(self.n)[act_set, :]
self.K[-6:, :] = self.constrMat
self.k1[-6:] = self.b
self.k2[-6:] = self.d
U, s, VT = np.linalg.svd(self.K)
rank = (s > EPS).sum()
K_inv_k1 = np.dot(VT[:rank, :].T,
(1.0 / s[:rank]) * np.dot(U[:, :rank].T, self.k1))
K_inv_k2 = np.dot(VT[:rank, :].T,
(1.0 / s[:rank]) * np.dot(U[:, :rank].T, self.k2))
if (rank < self.n):
Z = VT[rank:, :].T
P = np.dot(
np.dot(Z, np.linalg.inv(np.dot(Z.T, np.dot(self.Hess, Z)))),
Z.T)
K_inv_k1 -= np.dot(P, np.dot(self.Hess, K_inv_k1))
K_inv_k2 -= np.dot(P,
np.dot(self.Hess, K_inv_k2) + self.grad)
# Check that the solution you get by solving the KKT is the same found by the solver
x_kkt = K_inv_k1 * self.alpha + K_inv_k2
if (norm(self.x - x_kkt) > 10 * EPS):
warnings.warn("[%s] ERROR x different from x_kkt. x=" %
(self.name) + str(self.x) + "\nx_kkt=" + str(x_kkt) +
" " + str(norm(self.x - x_kkt)))
# store the derivative of the solution w.r.t. the parameter alpha
dx = K_inv_k1[-1]
# act_set_mat * alpha >= act_set_vec
act_set_mat = K_inv_k1[:-1]
act_set_vec = -K_inv_k2[:-1]
for i in range(act_set_mat.shape[0]):
if (abs(act_set_mat[i]) > EPS):
act_set_vec[i] /= abs(act_set_mat[i])
act_set_mat[i] /= abs(act_set_mat[i])
if (act_set_mat * self.alpha < act_set_vec - EPS).any():
raise ValueError(
"ERROR: after normalization current alpha violates constraints "
+ str(act_set_mat * self.alpha - act_set_vec))
ind_pos = np.where(act_set_mat > EPS)[0]
if (ind_pos.shape[0] > 0):
alpha_min = np.max(act_set_vec[ind_pos])
else:
alpha_min = -1e10
# warnings.warn("[%s] alpha_min seems unbounded %.7f"%(self.name, np.max(act_set_mat)));
ind_neg = np.where(act_set_mat < -EPS)[0]
if (ind_neg.shape[0] > 0):
alpha_max = np.min(-act_set_vec[ind_neg])
else:
alpha_max = 1e10
# warnings.warn("[%s] alpha_max seems unbounded %.7f"%(self.name, np.min(act_set_mat)));
if (alpha_min > alpha_max):
raise ValueError("ERROR alpha_min %.3f > alpha_max %.3f" %
(alpha_min, alpha_max))
return (dx, alpha_min, alpha_max)
def compute_max_deceleration(self, alpha, maxIter=None, maxTime=100.0):
start = time.time()
self.alpha = alpha
if (self.NO_WARM_START):
self.qpOasesSolver = SQProblem(self.n, self.m_in)
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
if (maxIter == None):
maxIter = self.maxIter
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
self.constrUB[:6] = np.dot(self.b, alpha) + self.d
self.constrLB[:6] = self.constrUB[:6]
while (True):
if (not self.initialized):
self.imode = self.qpOasesSolver.init(self.Hess, self.grad,
self.constrMat, self.lb,
self.ub, self.constrLB,
self.constrUB,
maxActiveSetIter,
maxComputationTime)
else:
self.imode = self.qpOasesSolver.hotstart(
self.grad, self.lb, self.ub, self.constrLB, self.constrUB,
maxActiveSetIter, maxComputationTime)
if (self.imode == 0):
self.initialized = True
if (self.imode == 0
or self.imode == PyReturnValue.INIT_FAILED_INFEASIBILITY or
self.imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY
or self.Hess[0, 0] >= MAX_HESSIAN_REGULARIZATION):
break
self.initialized = False
self.Hess *= 10.0
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
if (self.verb > -1):
print "[%s] WARNING %s. Increasing Hessian regularization to %f" % (
self.name, qpOasesSolverMsg(self.imode), self.Hess[0, 0])
self.qpTime = maxComputationTime
self.iter = 1 + maxActiveSetIter[0]
if (self.imode == 0):
self.qpOasesSolver.getPrimalSolution(self.x)
self.qpOasesSolver.getDualSolution(self.y)
if ((self.x < self.lb - self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError("[%s] ERROR lower bound violated" %
(self.name) + str(self.x) + str(self.lb))
if ((self.x > self.ub + self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError("[%s] ERROR upper bound violated" %
(self.name) + str(self.x) + str(self.ub))
if ((np.dot(self.constrMat, self.x) >
self.constrUB + self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError(
"[%s] ERROR constraint upper bound violated " %
(self.name) +
str(np.min(np.dot(self.constrMat, self.x) -
self.constrUB)))
if ((np.dot(self.constrMat, self.x) <
self.constrLB - self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError(
"[%s] ERROR constraint lower bound violated " %
(self.name) +
str(np.max(np.dot(self.constrMat, self.x) -
self.constrLB)))
(dx, alpha_min,
alpha_max) = self.compute_max_deceleration_derivative()
else:
self.initialized = False
dx = 0.0
alpha_min = 0.0
alpha_max = 0.0
if (self.verb > 0):
print "[%s] ERROR Qp oases %s" % (self.name,
qpOasesSolverMsg(self.imode))
if (self.qpTime >= maxTime):
if (self.verb > 0):
print "[%s] Max time reached %f after %d iters" % (
self.name, self.qpTime, self.iter)
self.imode = 9
self.computationTime = time.time() - start
return (self.imode, self.x[-1], dx, alpha_min, alpha_max)
def getContactForces(self):
''' Get the contact forces obtained by solving the last LP '''
cg = 4
nContacts = self.G4.shape[1] / cg
f = np.empty((3, nContacts))
for i in range(nContacts):
f[:, i] = np.dot(self.G4[:, cg * i:cg * i + cg],
self.x[cg * i:cg * i + cg])
return f
def reset(self):
self.initialized = False
ub_new = np.array([5.0, -0.5])
lbA_new = np.array([-2.0])
ubA_new = np.array([1.0])
# Setting up QProblem object.
example = QProblem(2, 1)
options = Options()
#options.printLevel = PrintLevel.NONE
example.setOptions(options)
# Solve first QP.
nWSR = np.array([10])
example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
xOpt = np.zeros(2)
example.getPrimalSolution(xOpt)
print("\nxOpt = [ %e, %e ]; objVal = %e\n\n" %
(xOpt[0], xOpt[1], example.getObjVal()))
# Solve second QP.
nWSR = np.array([10])
example.hotstart(g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR)
# Get and print solution of second QP.
example.getPrimalSolution(xOpt)
print("\nxOpt = [ %e, %e ]; objVal = %e\n\n" %
(xOpt[0], xOpt[1], example.getObjVal()))
example.printOptions()
def test_example1(self):
return 0
# Example for qpOASES main function using the QProblem class.
#Setup data of first QP.
H = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
A = np.array([1.0, 1.0 ]).reshape((2,1))
g = np.array([1.5, 1.0 ])
lb = np.array([0.5, -2.0])
ub = np.array([5.0, 2.0 ])
lbA = np.array([-1.0 ])
ubA = np.array([2.0])
# Setup data of second QP.
g_new = np.array([1.0, 1.5])
lb_new = np.array([0.0, -1.0])
ub_new = np.array([5.0, -0.5])
lbA_new = np.array([-2.0])
ubA_new = np.array([1.0])
# Setting up QProblemB object.
qp = QProblem(2, 1)
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 10
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
# Solve second QP.
nWSR = 10
qp.hotstart(g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR)
# Get and print solution of second QP.
xOpt_actual = np.zeros(2)
qp.getPrimalSolution(xOpt_actual)
xOpt_actual = np.asarray(xOpt_actual, dtype=float)
objVal_actual = qp.getObjVal()
objVal_actual = np.asarray(objVal_actual, dtype=float)
cmd = os.path.join(bin_path, "example1")
p = Popen(cmd, shell=True, stdout=PIPE)
stdout, stderr = p.communicate()
stdout = str(stdout).replace('\\n', '\n')
stdout = stdout.replace("'", '')
print(stdout)
# get c++ solution from std
pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
match = pattern.search(stdout)
xOpt_expected = match.group('xOpt')
xOpt_expected = xOpt_expected.split(",")
xOpt_expected = np.asarray(xOpt_expected, dtype=float)
pattern = re.compile(r'objVal = (?P<objVal>[0-9-+e.]*)')
match = pattern.search(stdout)
objVal_expected = match.group('objVal')
objVal_expected = np.asarray(objVal_expected, dtype=float)
print("xOpt_actual =", xOpt_actual)
print("xOpt_expected =", xOpt_expected)
print("objVal_actual = ", objVal_actual)
print("objVal_expected = ", objVal_expected)
assert_almost_equal(xOpt_actual, xOpt_expected, decimal=7)
assert_almost_equal(objVal_actual, objVal_expected, decimal=7)
lbA_new = np.array([-2.0])
ubA_new = np.array([1.0])
# Setting up QProblem object.
example = QProblem(2, 1)
options = Options()
options.printLevel = PrintLevel.NONE
example.setOptions(options)
# Solve first QP.
nWSR = np.array([10])
example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
# Solve second QP.
nWSR = np.array([10])
for i in range(100000):
for j in range(1, 100):
g_new[0] = i%j
example.hotstart( g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR)
# Get and print solution of second QP.
xOpt = np.zeros(2)
example.getPrimalSolution(xOpt)
print("\nxOpt = [ %e, %e ]; objVal = %e\n\n"%(xOpt[0],xOpt[1],example.getObjVal()))
example.printOptions()
