def test_example7(self):
H = np.array([ 0.8514828085899353, -0.15739890933036804, -0.081726007163524628, -0.530426025390625, 0.16773293912410736,
-0.15739890933036804, 1.1552412509918213, 0.57780224084854126, -0.0072606131434440613, 0.010559185408055782,
-0.081726007163524628, 0.57780224084854126, 0.28925251960754395, 5.324830453901086e-006, -3.0256599075073609e-006,
-0.530426025390625, -0.0072606131434440613, 5.324830453901086e-006, 0.35609596967697144, -0.15124998986721039,
0.16773293912410736, 0.010559185408055782, -3.0256599075073609e-006, -0.15124998986721039,
0.15129712224006653], dtype=float).reshape((5, 5))
g = np.array([0.30908384919166565, 0.99325823783874512, 0.49822014570236206, -0.26309865713119507, 0.024296050891280174], dtype=float).reshape((5,))
A = np.array([1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1], dtype=float).reshape((5, 5))
lb = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
ub = np.array([ 0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))
lbA = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
ubA = np.array([0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))
# Setting up QProblem object.
qp = QProblem(5, 5)
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 100
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
result = np.zeros((5,))
qp.getPrimalSolution(result)
def __init__(self, constraint_list, path, path_discretization):
assert qpoases_FOUND, "toppra is unable to find any installation of qpoases!"
super(qpOASESSolverWrapper, self).__init__(
constraint_list, path, path_discretization
)
# Currently only support Canonical Linear Constraint
self.nC = 2 # First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u
for i, constraint in enumerate(constraint_list):
if constraint.get_constraint_type() != ConstraintType.CanonicalLinear:
raise NotImplementedError
a, b, c, F, v, _, _ = self.params[i]
if a is not None:
if constraint.identical:
self.nC += F.shape[0]
else:
self.nC += F.shape[1]
self._A = np.zeros((self.nC, self.nV))
self._lA = -np.ones(self.nC)
self._hA = -np.ones(self.nC)
option = Options()
option.printLevel = PrintLevel.NONE
self.solver = SQProblem(self.nV, self.nC)
self.solver.setOptions(option)
def get_dq(self, q, e1, J1, e2, J2, e3, J3, e4, J4):
de1 = self.lambda1 * e1
de2 = self.lambda2 * e2
de3 = self.lambda3 * e3
de4 = self.lambda4 * e4
W = self.w1 * np.dot(J1.T, J1) + self.w2 * np.dot(
J2.T, J2) + self.w3 * np.dot(J3.T, J3) + self.w4 * np.dot(
J4.T, J4)
p = -2 * (self.w1 * np.dot(J1.T, de1) + self.w2 * np.dot(J2.T, de2) +
self.w3 * np.dot(J3.T, de3) + self.w4 * np.dot(J4.T, de4))
lower_limits = np.maximum((self.qmin - q[7:]) / self.dt, self.dqmin)
upper_limits = np.minimum((self.qmax - q[7:]) / self.dt, self.dqmax)
lower_limits = np.hstack((self.lfb, lower_limits))
upper_limits = np.hstack((self.ufb, upper_limits))
# Solver
solver = QProblemB(19)
options = Options()
options.setToMPC()
options.printLevel = PrintLevel.LOW
solver.setOptions(options)
nWSR = np.array([10])
solver.init(W, p, lower_limits, upper_limits, nWSR)
dq = np.zeros(19)
solver.getPrimalSolution(dq)
return dq
def fit(self, X):
q, n = len(self.alphas), X.shape[0]
self.X = X
if self.kernel == 'gauss':
H = self.kernel_fun(X, gamma=self.gamma)
else:
H = self.kernel_fun(X, X)
H = 1 / 2 * (H + np.transpose(H))
H = repmat(H, q, q)
g = np.zeros(q * n)
A = np.zeros((q, q * n))
lbA = np.ones(q)
lb = np.zeros(q * n)
ub = np.ones(q * n)
for i in range(q):
start = i * n + 1
end = start + n - 1
ub[start:end] = 1 / (n * (1 - self.alphas[i]))
A[i, start:end] = 1
qp = QProblem(q * n, q)
if not self.verbose:
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
suc = qp.init(H, g, A, lb, ub, lbA, lbA, self.max_iter)
if suc == ReturnValue.MAX_NWSR_REACHED:
msg = "qPOASES reached the maximum number of iterations ({}). ".format(
self.max_iter)
msg += "\nThe resulting regions may not be reliable"
print(msg)
etas = np.zeros(q * n)
qp.getPrimalSolution(etas)
etas = etas.reshape(q, n)
etastars = etas.sum(axis=0)
nus = 1 - self.alphas
SVidx = np.arange(len(etastars))[etastars > self.tol]
nSV = len(SVidx)
ub = 1 / n * nus
rhos = np.zeros(q)
for j, eta in enumerate(etas):
choose = np.logical_and(eta > self.tol, eta < ub[j])
hyperSVidx = np.arange(len(eta))[choose]
if len(hyperSVidx) == 0:
hyperSVidx = np.arange(len(eta))[eta > self.tol]
rhos[j] = max(
np.dot(H[hyperSVidx][:, SVidx], etastars[SVidx]) / q)
else:
rhos[j] = np.median(
np.dot(H[hyperSVidx][:, SVidx], etastars[SVidx]) / q)
self.rhos = rhos
self.etastars = etastars
tmp = np.dot(H[:, SVidx], etastars[SVidx]) / q
self.rhobounds = tmp.max(), tmp.min()
return self
def solveLeastSquare(A,
b,
lb=None,
ub=None,
A_in=None,
lb_in=None,
ub_in=None):
'''
Solve the least square problem:
minimize || A*x-b ||^2
subject to lb_in <= A_in*x <= ub_in
lb <= x <= ub
'''
n = A.shape[1]
m_in = 0
if A_in is not None:
m_in = A_in.shape[0]
if lb_in is None:
lb_in = np.array(m_in * [-1e99])
if ub_in is None:
ub_in = np.array(m_in * [1e99])
if lb is None:
lb = np.array(n * [-1e99])
if ub is None:
ub = np.array(n * [1e99])
Hess = np.dot(A.transpose(), A)
grad = -np.dot(A.transpose(), b)
maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n])
maxComputationTime = np.array([600.0])
options = Options()
options.printLevel = PrintLevel.LOW
# NONE, LOW, MEDIUM
options.enableRegularisation = True
print('Gonna solve QP...')
if m_in == 0:
qpOasesSolver = QProblemB(n)
# , HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter,
maxComputationTime)
else:
qpOasesSolver = SQProblem(n, m_in)
# , HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in,
maxActiveSetIter, maxComputationTime)
print('QP solved in %f seconds and %d iterations' %
(maxComputationTime[0], maxActiveSetIter[0]))
if imode != 0 and imode != 63:
print("ERROR Qp oases %d " % (imode))
x_norm = np.zeros(n)
# solution of the normalized problem
qpOasesSolver.getPrimalSolution(x_norm)
return x_norm
def setup_solver(self):
option = Options()
if logger.getEffectiveLevel() == logging.DEBUG:
option.printLevel = PrintLevel.HIGH
else:
option.printLevel = PrintLevel.NONE
self.solver_up = SQProblem(self.nV, self.nC)
self.solver_up.setOptions(option)
self.solver_down = SQProblem(self.nV, self.nC)
self.solver_down.setOptions(option)
self.solver_up_recent_index = -2
self.solver_down_recent_index = -2
def solveLeastSquare(A,
b,
lb=None,
ub=None,
A_in=None,
lb_in=None,
ub_in=None):
n = A.shape[1]
m_in = 0
if (A_in != None):
m_in = A_in.shape[0]
if (lb_in == None):
lb_in = np.array(m_in * [-1e99])
if (ub_in == None):
ub_in = np.array(m_in * [1e99])
if (lb == None):
lb = np.array(n * [-1e99])
if (ub == None):
ub = np.array(n * [1e99])
Hess = np.dot(A.transpose(), A)
grad = -np.dot(A.transpose(), b)
maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n])
maxComputationTime = np.array([600.0])
options = Options()
options.printLevel = PrintLevel.LOW
#NONE, LOW, MEDIUM
options.enableRegularisation = True
print 'Gonna solve QP...'
if (m_in == 0):
qpOasesSolver = QProblemB(n)
#, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter,
maxComputationTime)
else:
qpOasesSolver = SQProblem(n, m_in)
#, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in,
maxActiveSetIter, maxComputationTime)
print 'QP solved in %f seconds and %d iterations' % (maxComputationTime[0],
maxActiveSetIter[0])
if (imode != 0 and imode != 63):
print "ERROR Qp oases %d " % (imode)
x_norm = np.zeros(n)
# solution of the normalized problem
qpOasesSolver.getPrimalSolution(x_norm)
return x_norm
def setup_solver(self):
"""Initiate two internal solvers for warm-start.
"""
option = Options()
if logger.getEffectiveLevel() == logging.DEBUG:
# option.printLevel = PrintLevel.HIGH
option.printLevel = PrintLevel.NONE
else:
option.printLevel = PrintLevel.NONE
self.solver_minimizing = SQProblem(self.nV, self.nC)
self.solver_minimizing.setOptions(option)
self.solver_maximizing = SQProblem(self.nV, self.nC)
self.solver_maximizing.setOptions(option)
self.solver_minimizing_recent_index = -2
self.solver_maximizing_recent_index = -2
def changeInequalityNumber(self, m_in):
# print "[%s] Changing number of inequality constraints from %d to %d" % (self.name, self.m_in, m_in);
if (m_in == self.m_in):
return
self.m_in = m_in
self.iter = 0
self.qpOasesSolver = SQProblem(self.n, m_in)
#, HessianType.POSDEF SEMIDEF
self.options = Options()
self.options.setToReliable()
if (self.verb <= 0):
self.options.printLevel = PrintLevel.NONE
elif (self.verb == 1):
self.options.printLevel = PrintLevel.LOW
elif (self.verb == 2):
self.options.printLevel = PrintLevel.MEDIUM
elif (self.verb > 2):
self.options.printLevel = PrintLevel.DEBUG_ITER
print "set high print level"
# self.options.enableRegularisation = False;
# self.options.enableFlippingBounds = BooleanType.FALSE
# self.options.initialStatusBounds = SubjectToStatus.INACTIVE
# self.options.setToMPC();
# self.qpOasesSolver.printOptions();
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
def __initSolver(self, arg_dim, num_in):
self.solver = SQProblem(arg_dim, num_in)
# , HessianType.POSDEF SEMIDEF
self.options = Options()
self.options.setToReliable()
self.solver.setOptions(self.options)
def __init__(self,
name,
maxIter=1000,
maxTime=100.0,
useWarmStart=True,
verb=0):
solver_LP_abstract.SolverLPAbstract.__init__(self, name, maxIter,
maxTime, useWarmStart,
verb)
self._hessian_regularization = DEFAULT_HESSIAN_REGULARIZATION
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._n = -1
self._m_con = -1
self._qpOasesSolver = None
def test_m44_reliable_sparse(self):
test_name = 'mm44_reliable_sparse.txt'
print(test_name)
# QP Options
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.NONE
isSparse = True
useHotstarts = False
# run QP benchmarks
results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
self.nWSR, self.cpu_time, self.TOL)
# print and write results
string = results2str(results)
print(string)
write_results(test_name, string)
assert get_nfail(results) <= 0, 'One ore more tests failed.'
def test_m44_reliable_sparse(self):
test_name = 'mm44_reliable_sparse.txt'
print(test_name)
# QP Options
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.NONE
isSparse = True
useHotstarts = False
# run QP benchmarks
results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
self.nWSR, self.cpu_time, self.TOL)
# print and write results
string = results2str(results)
print(string)
write_results(test_name, string)
assert get_nfail(results) <= 0, 'One ore more tests failed.'
def test_id_hessian(self):
"""Very simple example for testing qpOASES (using QProblem class)."""
path = os.path.join(testing_path, "dev_idhessian_data")
#Setup data for QP.
H = np.loadtxt(os.path.join(path, "H.txt"))
g = np.loadtxt(os.path.join(path, "g.txt"))
A = np.loadtxt(os.path.join(path, "A.txt"))
lb = np.loadtxt(os.path.join(path, "lb.txt"))
ub = np.loadtxt(os.path.join(path, "ub.txt"))
lbA = np.loadtxt(os.path.join(path, "lbA.txt"))
ubA = np.loadtxt(os.path.join(path, "ubA.txt"))
#Setting up QProblem object.
qp = QProblem(72,144)
options = Options()
options.numRefinementSteps = 1
options.printLevel = PrintLevel.NONE
#options.setToMPC()
#options.setToReliable()
#options.enableFlippingBounds = BooleanType.FALSE
options.enableRamping = BooleanType.FALSE
#options.enableRamping = BooleanType.TRUE
#options.enableFarBounds = BooleanType.FALSE
#options.enableRamping = BooleanType.FALSE
#options.printLevel = PL_LOW
#options.enableFullLITests = BooleanType.FALSE
#options.boundRelaxation = 1.0e-1
qp.setOptions( options )
#Solve QP.
nWSR = 1200
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
def test_id_hessian(self):
"""Very simple example for testing qpOASES (using QProblem class)."""
path = os.path.join(testing_path, "dev_idhessian_data")
#Setup data for QP.
H = np.loadtxt(os.path.join(path, "H.txt"))
g = np.loadtxt(os.path.join(path, "g.txt"))
A = np.loadtxt(os.path.join(path, "A.txt"))
lb = np.loadtxt(os.path.join(path, "lb.txt"))
ub = np.loadtxt(os.path.join(path, "ub.txt"))
lbA = np.loadtxt(os.path.join(path, "lbA.txt"))
ubA = np.loadtxt(os.path.join(path, "ubA.txt"))
#Setting up QProblem object.
qp = QProblem(72, 144)
options = Options()
options.numRefinementSteps = 1
options.printLevel = PrintLevel.NONE
#options.setToMPC()
#options.setToReliable()
#options.enableFlippingBounds = BooleanType.FALSE
options.enableRamping = BooleanType.FALSE
#options.enableRamping = BooleanType.TRUE
#options.enableFarBounds = BooleanType.FALSE
#options.enableRamping = BooleanType.FALSE
#options.printLevel = PL_LOW
#options.enableFullLITests = BooleanType.FALSE
#options.boundRelaxation = 1.0e-1
qp.setOptions(options)
#Solve QP.
nWSR = 1200
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
def solveLeastSquare(A, b, lb=None, ub=None, A_in=None, lb_in=None, ub_in=None):
n = A.shape[1];
m_in = 0;
if(A_in!=None):
m_in = A_in.shape[0];
if(lb_in==None):
lb_in = np.array(m_in*[-1e99]);
if(ub_in==None):
ub_in = np.array(m_in*[1e99]);
if(lb==None):
lb = np.array(n*[-1e99]);
if(ub==None):
ub = np.array(n*[1e99]);
Hess = np.dot(A.transpose(),A);
grad = -np.dot(A.transpose(),b);
maxActiveSetIter = np.array([100+2*m_in+2*n]);
maxComputationTime = np.array([600.0]);
options = Options();
options.printLevel = PrintLevel.LOW; #NONE, LOW, MEDIUM
options.enableRegularisation = True;
print 'Gonna solve QP...';
if(m_in==0):
qpOasesSolver = QProblemB(n); #, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options);
imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter, maxComputationTime);
else:
qpOasesSolver = SQProblem(n, m_in); #, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options);
imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in, maxActiveSetIter, maxComputationTime);
# print 'QP solved in %f seconds and %d iterations' % (maxComputationTime[0],maxActiveSetIter[0]);
if(imode!=0 and imode!=63):
print "ERROR Qp oases %d " % (imode);
x_norm = np.zeros(n); # solution of the normalized problem
qpOasesSolver.getPrimalSolution(x_norm);
return x_norm;
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)
class AbstractSolver(object):
"""
Abstract solver class to use as base for all solvers. The basic problem has
the following structure:
minimize 0.5*||D*x - d||^2
subject to Alb <= A*x <= Aub
lb <= x <= ub
NB: Before it was:
subject to G*x + g >= 0
where
"""
NO_WARM_START = False
name = ""
# solver name
n = 0
# number of variables
m_in = -1
# number of inequalities
D = []
# quadratic cost matrix
d = []
# quadratic cost vector
G = []
# inequality matrix
g = []
# inequality vector
bounds = []
# bounds on the problem variables
H = []
# Hessian H = G^T*G
dD = []
# Product dD = d^T*D
x0 = []
# initial guess
solver = ''
# type of solver to use
accuracy = 0
# accuracy used by the solver for termination
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
iterationNumber = 0
# total number of iterations
approxProb = 0
# approximated probability of G*x+g>0
initialized = False
# true if solver has been initialized
nActiveInequalities = 0
# number of active inequality constraints
nViolatedInequalities = 0
# number of violated inequalities
outerIter = 0
# number of outer (Newton) iterations
qpOasesSolver = []
options = []
# qp oases solver's options
softInequalityIndexes = []
epsilon = np.sqrt(np.finfo(float).eps)
INEQ_VIOLATION_THR = 1e-4
def __init__(self,
n,
m_in,
solver='slsqp',
accuracy=1e-6,
maxIter=100,
verb=0):
self.name = "AbstractSolver"
self.n = n
self.iter = 0
self.solver = solver
self.accuracy = accuracy
self.maxIter = maxIter
self.verb = verb
self.qpOasesAnalyser = SolutionAnalysis()
self.changeInequalityNumber(m_in)
return
def setSoftInequalityIndexes(self, indexes):
self.softInequalityIndexes = indexes
def changeInequalityNumber(self, m_in):
# print "[%s] Changing number of inequality constraints from %d to %d" % (self.name, self.m_in, m_in);
if (m_in == self.m_in):
return
self.m_in = m_in
self.iter = 0
self.qpOasesSolver = SQProblem(self.n, m_in)
#, HessianType.POSDEF SEMIDEF
self.options = Options()
self.options.setToReliable()
if (self.verb <= 0):
self.options.printLevel = PrintLevel.NONE
elif (self.verb == 1):
self.options.printLevel = PrintLevel.LOW
elif (self.verb == 2):
self.options.printLevel = PrintLevel.MEDIUM
elif (self.verb > 2):
self.options.printLevel = PrintLevel.DEBUG_ITER
print "set high print level"
# self.options.enableRegularisation = False;
# self.options.enableFlippingBounds = BooleanType.FALSE
# self.options.initialStatusBounds = SubjectToStatus.INACTIVE
# self.options.setToMPC();
# self.qpOasesSolver.printOptions();
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
def setProblemData(self, D, d, A, lbA, ubA, lb, ub, x0=None):
self.D = D
self.d = d.squeeze()
if (A.shape[0] == self.m_in and A.shape[1] == self.n):
self.A = A
self.lbA = lbA.squeeze()
self.ubA = ubA.squeeze()
else:
print "[%s] ERROR. Wrong size of the constraint matrix, %d rather than %d" % (
self.name, A.shape[0], self.m_in)
if (lb.shape[0] == self.n and ub.shape[0] == self.n):
self.lb = lb.squeeze()
self.ub = ub.squeeze()
else:
print "[%s] ERROR. Wrong size of the bound vectors, %d and %d rather than %d" % (
self.name, lb.shape[0], ub.shape[0], self.n)
# self.bounds = self.n*[(-1e10,1e10)];
if (x0 is None):
self.x0 = np.zeros(self.n)
else:
self.x0 = x0.squeeze()
self.H = np.dot(self.D.T, self.D)
self.dD = np.dot(self.D.T, self.d)
def solve(self,
D,
d,
A,
lbA,
ubA,
lb,
ub,
x0=None,
maxIter=None,
maxTime=100.0):
if (self.NO_WARM_START):
self.qpOasesSolver = SQProblem(self.n, self.m_in)
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
if (maxIter is None):
maxIter = self.maxIter
self.iter = 0
self.qpTime = 0.0
self.removeSoftInequalities = False
self.setProblemData(D, d, A, lbA, ubA, lb, ub, x0)
start = time.time()
x = np.zeros(self.x0.shape)
if (self.solver == 'slsqp'):
(x, fx, self.iterationNumber, imode,
smode) = fmin_slsqp(self.f_cost,
self.x0,
fprime=self.f_cost_grad,
f_ieqcons=self.f_inequalities,
fprime_ieqcons=self.f_inequalities_jac,
bounds=self.bounds,
iprint=self.verb,
iter=maxIter,
acc=self.accuracy,
full_output=1)
self.fx = fx
x = np.array(x)
''' Exit modes are defined as follows :
-1 : Gradient evaluation required (g & a)
0 : Optimization terminated successfully.
1 : Function evaluation required (f & c)
2 : More equality constraints than independent variables
3 : More than 3*n iterations in LSQ subproblem
4 : Inequality constraints incompatible
5 : Singular matrix E in LSQ subproblem
6 : Singular matrix C in LSQ subproblem
7 : Rank-deficient equality constraint subproblem HFTI
8 : Positive directional derivative for linesearch
9 : Iteration limit exceeded
'''
if (self.verb > 0 and imode != 0 and imode !=
9): #do not print error msg if iteration limit exceeded
print "[%s] *** ERROR *** %s" % (self.name, smode)
elif (self.solver == 'qpoases'):
# ubA = np.array(self.m_in*[1e9]);
# lb = np.array([ b[0] for b in self.bounds]);
# ub = np.array([ b[1] for b in self.bounds]);
# A = self.get_linear_inequality_matrix();
self.iter = 0
#total iters of qpoases
# lbA = -self.get_linear_inequality_vector();
Hess = self.f_cost_hess(x)
grad = self.f_cost_grad(x)
self.fx = self.f_cost(x)
maxActiveSetIter = np.array([maxIter - self.iter])
maxComputationTime = np.array(maxTime)
if (self.initialized == False):
imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb,
self.ub, self.lbA, self.ubA,
maxActiveSetIter,
maxComputationTime)
if (imode == 0):
self.initialized = True
else:
imode = self.qpOasesSolver.hotstart(Hess, grad, self.A,
self.lb, self.ub, self.lbA,
self.ubA, maxActiveSetIter,
maxComputationTime)
if (imode ==
PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED):
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
imode = self.qpOasesSolver.init(Hess, grad, self.A,
self.lb, self.ub, self.lbA,
self.ubA, maxActiveSetIter,
maxComputationTime)
if (imode == 0):
self.initialized = True
self.qpTime += maxComputationTime
self.iter = 1 + maxActiveSetIter[0]
self.iterationNumber = self.iter
''' if the solution found is unfeasible check whether the initial guess is feasible '''
self.qpOasesSolver.getPrimalSolution(x)
ineq_marg = self.f_inequalities(x)
qpUnfeasible = False
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
qpUnfeasible = True
if (x0 is not None):
ineq_marg = self.f_inequalities(x0)
if (not (ineq_marg < -self.INEQ_VIOLATION_THR).any()):
if (self.verb > 0):
print "[%s] Solution found is unfeasible but initial guess is feasible" % (
self.name)
qpUnfeasible = False
x = np.copy(x0)
''' if both the solution found and the initial guess are unfeasible remove the soft constraints '''
if (qpUnfeasible and len(self.softInequalityIndexes) > 0):
# remove soft inequality constraints and try to solve again
self.removeSoftInequalities = True
maxActiveSetIter[0] = maxIter
lbAsoft = np.copy(self.lbA)
ubAsoft = np.copy(self.ubA)
lbAsoft[self.softInequalityIndexes] = -1e100
ubAsoft[self.softInequalityIndexes] = 1e100
imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb,
self.ub, lbAsoft, ubAsoft,
maxActiveSetIter)
self.qpOasesSolver.getPrimalSolution(x)
ineq_marg = self.f_inequalities(x)
ineq_marg[self.softInequalityIndexes] = 1.0
qpUnfeasible = False
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
''' if the solution found is unfeasible check whether the initial guess is feasible '''
if (x0 is not None):
x = np.copy(x0)
ineq_marg = self.f_inequalities(x)
ineq_marg[self.softInequalityIndexes] = 1.0
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
print "[%s] WARNING Problem unfeasible even without soft constraints" % (
self.name), np.min(ineq_marg), imode
qpUnfeasible = True
elif (self.verb > 0):
print "[%s] Initial guess is feasible for the relaxed problem" % (
self.name)
else:
print "[%s] No way to get a feasible solution (no initial guess)" % (
self.name), np.min(ineq_marg)
elif (self.verb > 0):
print "[%s] Solution found and initial guess are unfeasible, but relaxed problem is feasible" % (
self.name)
if (qpUnfeasible):
self.print_qp_oases_error_message(imode, self.name)
if (self.verb > 1):
activeIneq = np.count_nonzero(np.abs(ineq_marg) < 1e-3)
print "[%s] Iter %d, active inequalities %d" % (
self.name, self.iter, activeIneq)
# termination conditions
if (self.iter >= maxIter):
imode = 9
if (self.verb > 1):
print "[%s] Max number of iterations reached %d" % (
self.name, self.iter)
if (self.qpTime >= maxTime):
print "[%s] Max time reached %f after %d iters" % (
self.name, self.qpTime, self.iter)
imode = 9
elif (self.solver == 'sqpoases'):
ubA = np.array(self.m_in * [1e99])
x_newton = np.zeros(x.shape)
x = self.x0
A = self.get_linear_inequality_matrix()
self.iter = 0
#total iters of qpoases
self.outerIter = 0
# number of outer (Newton) iterations
while True:
# compute Newton step
lb = np.array([b[0] for b in self.bounds])
ub = np.array([b[1] for b in self.bounds])
lb -= x
ub -= x
lbA = -np.dot(A, x) - self.get_linear_inequality_vector()
# if(self.outerIter>0):
# if((lbA>0.0).any()):
# print "[%s] Iter %d lbA[%d]=%f"%(self.name,self.outerIter,np.argmax(lbA),np.max(lbA));
Hess = self.f_cost_hess(x)
grad = self.f_cost_grad(x)
self.fx = self.f_cost(x)
maxActiveSetIter = np.array([maxIter - self.iter])
maxComputationTime = np.array(maxTime)
if (self.initialized == False):
imode = self.qpOasesSolver.init(Hess, grad, A, lb, ub, lbA,
ubA, maxActiveSetIter,
maxComputationTime)
if (imode == 0):
self.initialized = True
else:
imode = self.qpOasesSolver.hotstart(
Hess, grad, A, lb, ub, lbA, ubA, maxActiveSetIter,
maxComputationTime)
self.qpTime += maxComputationTime
maxTime -= maxComputationTime
# count iterations
self.iter += 1 + maxActiveSetIter[0]
self.outerIter += 1
self.qpOasesSolver.getPrimalSolution(x_newton)
''' check feasibility of the constraints '''
x_new = x + x_newton
ineq_marg = self.f_inequalities(x_new)
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
if (len(self.softInequalityIndexes) > 0
and self.outerIter == 1):
''' if constraints are unfeasible at first iteration remove soft inequalities '''
if (self.verb > 1):
print '[%s] Remove soft constraints' % (self.name)
self.removeSoftInequalities = True
self.g[self.softInequalityIndexes] = 1e100
self.iter = 0
continue
elif (len(self.softInequalityIndexes) > 0
and self.removeSoftInequalities == False):
''' if constraints are unfeasible at later iteration remove soft inequalities '''
if (self.verb >= 0):
print '[%s] Remove soft constraints at iter %d' % (
self.name, self.outerIter)
self.removeSoftInequalities = True
self.g[self.softInequalityIndexes] = 1e100
continue
else:
if ((lbA > 0.0).any()):
print "[%s] WARNING Problem unfeasible (even without soft constraints) %f" % (
self.name, np.max(lbA)), imode
if (self.verb > 0):
''' qp failed for some reason (e.g. max iter) '''
print "[%s] WARNING imode %d ineq unfeasible iter %d: %f, max(lbA)=%f" % (
self.name, imode, self.outerIter,
np.min(ineq_marg), np.max(lbA))
break
''' if constraints are unfeasible at later iterations exit '''
if (imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY):
print "[%s] Outer iter %d QPoases says problem is unfeasible but constraints are satisfied: %f" % (
self.name, self.outerIter, np.min(ineq_marg))
ind = np.where(ineq_marg < 0.0)[0]
self.g[ind] -= ineq_marg[ind]
continue
self.print_qp_oases_error_message(imode, self.name)
# check for convergence
newton_dec_squared = np.dot(x_newton, np.dot(Hess, x_newton))
if (0.5 * newton_dec_squared < self.accuracy):
ineq_marg = self.f_inequalities(x)
if ((ineq_marg >= -self.INEQ_VIOLATION_THR).all()):
if (self.verb > 0):
print "[%s] Optimization converged in %d steps" % (
self.name, self.iter)
break
elif (self.verb > 0):
v = ineq_marg < -self.INEQ_VIOLATION_THR
print(
self.name, self.outerIter,
"WARNING Solver converged but inequalities violated:",
np.where(v), ineq_marg[v])
elif (self.verb > 1):
print "[%s] Optimization did not converge yet, squared Newton decrement: %f" % (
self.name, newton_dec_squared)
# line search
(alpha, fc, gc, phi, old_fval,
derphi) = line_search(self.f_cost,
self.f_cost_grad,
x,
x_newton,
grad,
self.fx,
c1=0.0001,
c2=0.9)
x_new = x + alpha * x_newton
new_fx = self.f_cost(x_new)
if (self.verb > 1):
print "[%s] line search alpha = %f, fc %d, gc %d, old cost %f, new cost %f" % (
self.name, alpha, fc, gc, self.fx, new_fx)
# Check that inequalities are still satisfied
ineq_marg = self.f_inequalities(x_new)
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
if (self.verb > 1):
print "[%s] WARNING some inequalities are violated with alpha=%f, gonna perform new line search." % (
self.name, alpha)
k = 2.0
for i in range(100):
alpha = min(k * alpha, 1.0)
x_new = x + alpha * x_newton
ineq_marg = self.f_inequalities(x_new)
if ((ineq_marg >= -self.INEQ_VIOLATION_THR).all()):
if (self.verb > 1):
print "[%s] With alpha=%f the solution satisfies the inequalities." % (
self.name, alpha)
break
if (alpha == 1.0):
print "[%s] ERROR With alpha=1 some inequalities are violated, error: %f" % (
self.name, np.min(ineq_marg))
break
x = x_new
if (self.verb > 1):
ineq_marg = self.f_inequalities(x)
activeIneq = np.count_nonzero(np.abs(ineq_marg) < 1e-3)
nViolIneq = np.count_nonzero(
ineq_marg < -self.INEQ_VIOLATION_THR)
print "[%s] Outer iter %d, iter %d, active inequalities %d, violated inequalities %d" % (
self.name, self.outerIter, self.iter, activeIneq,
nViolIneq)
# termination conditions
if (self.iter >= maxIter):
if (self.verb > 1):
print "[%s] Max number of iterations reached %d" % (
self.name, self.iter)
imode = 9
break
if (maxTime < 0.0):
print "[%s] Max time reached %.4f s after %d out iters, %d iters, newtonDec %.6f removeSoftIneq" % (
self.name, self.qpTime, self.outerIter, self.iter,
newton_dec_squared), self.removeSoftInequalities
imode = 9
break
self.iterationNumber = self.iter
else:
print '[%s] Solver type not recognized: %s' % (self.name,
self.solver)
return np.zeros(self.n)
self.computationTime = time.time() - start
ineq = self.f_inequalities(x)
if (self.removeSoftInequalities):
ineq[self.softInequalityIndexes] = 1.0
self.nViolatedInequalities = np.count_nonzero(
ineq < -self.INEQ_VIOLATION_THR)
self.nActiveInequalities = np.count_nonzero(ineq < 1e-3)
self.imode = imode
self.print_solution_info(x)
self.finalize_solution(x)
return (x, imode)
def finalize_solution(self, x):
pass
def f_cost(self, x):
e = np.dot(self.D, x) - self.d
return 0.5 * np.dot(e.T, e)
def f_cost_grad(self, x):
return approx_fprime(x, self.f_cost, self.epsilon)
def f_cost_hess(self, x):
return approx_jacobian(x, self.f_cost_grad, self.epsilon)
def get_linear_inequality_matrix(self):
return self.A
def get_linear_inequality_vectors(self):
return (self.lbA, self.ubA)
def f_inequalities(self, x):
ineq_marg = np.zeros(2 * self.m_in)
Ax = np.dot(self.get_linear_inequality_matrix(), x)
ineq_marg[:self.m_in] = Ax - self.lbA
ineq_marg[self.m_in:] = self.ubA - Ax
return ineq_marg
def f_inequalities_jac(self, x):
return self.get_linear_inequality_matrix()
def print_solution_info(self, x):
if (self.verb > 1):
print(self.name, "Solution is ", x)
def reset(self):
self.initialized = False
def check_grad(self, x=None):
if (x is None):
x = np.random.rand(self.n)
grad = self.f_cost_grad(x)
grad_fd = approx_fprime(x, self.f_cost, self.epsilon)
err = np.sqrt(sum((grad - grad_fd)**2))
print "[%s] Gradient error: %f" % (self.name, err)
return (grad, grad_fd)
def check_hess(self, x=None):
if (x is None):
x = np.random.rand(self.n)
hess = self.f_cost_hess(x)
hess_fd = approx_jacobian(x, self.f_cost_grad, self.epsilon)
err = np.sqrt(np.sum((hess - hess_fd)**2))
print "[%s] Hessian error: %f" % (self.name, err)
return (hess, hess_fd)
def print_qp_oases_error_message(self, imode, solver_name):
if (imode != 0 and imode != PyReturnValue.MAX_NWSR_REACHED):
if (imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY):
print "[%s] ERROR Qp oases HOTSTART_STOPPED_INFEASIBILITY" % solver_name
# 61
elif (imode == PyReturnValue.MAX_NWSR_REACHED):
print "[%s] ERROR Qp oases RET_MAX_NWSR_REACHED" % solver_name
# 64
elif (imode == PyReturnValue.STEPDIRECTION_FAILED_TQ):
print "[%s] ERROR Qp oases STEPDIRECTION_FAILED_TQ" % solver_name
# 68
elif (imode == PyReturnValue.STEPDIRECTION_FAILED_CHOLESKY):
print "[%s] ERROR Qp oases STEPDIRECTION_FAILED_CHOLESKY" % solver_name
# 69
elif (imode == PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED
):
print "[%s] ERROR Qp oases HOTSTART_FAILED_AS_QP_NOT_INITIALISED" % solver_name
# 54
elif (imode == PyReturnValue.INIT_FAILED_HOTSTART):
print "[%s] ERROR Qp oases INIT_FAILED_HOTSTART" % solver_name
# 36
elif (imode == PyReturnValue.INIT_FAILED_INFEASIBILITY):
print "[%s] ERROR Qp oases INIT_FAILED_INFEASIBILITY" % solver_name
# 37
elif (imode == PyReturnValue.UNKNOWN_BUG):
print "[%s] ERROR Qp oases UNKNOWN_BUG" % solver_name
# 9
else:
print "[%s] ERROR Qp oases %d " % (solver_name, imode)
def main():
rospy.init_node('controller_2dof', anonymous=True)
rospy.Rate(10)
# Setup data of QP.
# Joint Weights.
w1 = 1e-3
w2 = 1e-3
# Joint limits.
q0_max = 0.05
q1_max = 0.15
# Links
l1 = 0.1
l2 = 0.2
l3 = 0.25
# Initial joint values.
q0_init = q0 = -0.03
q1_init = q1 = 0.0
# Joint target.
q_des1 = 0.4
q_des2 = 0.7
q_des3 = 0.8
# Slack limits.
e1_max = 1000
e2_max = 1000
e3_max = 1000
# Velocity limits.(+-)
v0_max = 0.05
v1_max = 0.1
# Acceleration limits.(+-)
a0_max = 0.05 * 0.5
a1_max = 0.1 * 0.5
# Others
precision = 1e-3
p = 10
q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1
error1 = p * (q_des1 - q_eef)
error2 = p * (q_des2 - q_eef)
error3 = p * (q_des3 - q_eef)
vel_init = 0
nWSR = np.array([100])
# Dynamic goal weights
sum_error = abs(error1) + abs(error2) + abs(error3)
min_error = min(abs(error1), abs(error2), abs(error3))
max_error = max(abs(error1), abs(error2), abs(error3))
w3 = (1 - (abs(error1) - min_error) /
(max_error - min_error))**3 # goal 1 weight
w4 = (1 - (abs(error2) - min_error) /
(max_error - min_error))**3 # goal 2 weight
w5 = (1 - (abs(error3) - min_error) /
(max_error - min_error))**3 # goal 3 weight
# Acceleration
a0_const = (v0_max - a0_max) / v0_max
a1_const = (v1_max - a1_max) / v1_max
example = SQProblem(5, 7)
H = np.array([
w1, 0.0, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0,
0.0, 0.0, 0.0, w4, 0.0, 0.0, 0.0, 0.0, 0.0, w5
]).reshape((5, 5))
A = np.array([
1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0,
1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0
]).reshape((7, 5))
g = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
lb = np.array([-v0_max, -v1_max, -e1_max, -e2_max, -e3_max])
ub = np.array([v0_max, v1_max, e1_max, e2_max, e3_max])
lbA = np.array([
error1, error2, error3, (-q0_max - q0), (-q1_max - q0), -a0_max,
-a1_max
])
ubA = np.array(
[error1, error2, error3, (q0_max - q0), (q1_max - q0), a0_max, a1_max])
# Setting up QProblem object
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.LOW
example.setOptions(options)
Opt = np.zeros(5)
print(
"Init pos = %g, goal_1 = %g, goal_2 = %g, error_1 = %g, error_2 = %g, q0 =%g, q1 = %g\n"
% (q_eef, q_des1, q_des2, error1, error2, q0, q1))
print 'A = {}\n'.format(A)
i = 0
# Stopping conditions
limit1 = abs(error1)
limit2 = abs(error2)
limit3 = abs(error3)
lim = min([limit1, limit2, limit3])
diff1 = abs(error1)
diff2 = abs(error2)
diff3 = abs(error3)
diff = min([diff1, diff2, diff3])
# Plotting
t = np.array(i)
pos_eef = np.array(q_eef)
pos_0 = np.array(q0)
pos_1 = np.array(q1)
vel_0 = np.array(vel_init)
vel_1 = np.array(vel_init)
vel_eef = np.array(vel_init)
p_error = np.array(error1)
go_1 = np.array(q_des1)
go_2 = np.array(q_des2)
go_3 = np.array(q_des3)
return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Init of QP-Problem returned without success! ERROR MESSAGE: "
return -1
while not rospy.is_shutdown():
while (diff > 0.0009) and lim > precision and i < 400:
# Solve QP.
i += 1
nWSR = np.array([100])
lbA[0] = error1
lbA[1] = error2
lbA[2] = error3
lbA[3] = -q0_max - q0
lbA[4] = -q1_max - q1
lbA[5] = a0_const * Opt[0] - a0_max
lbA[6] = a1_const * Opt[1] - a1_max
ubA[0] = error1
ubA[1] = error2
ubA[2] = error3
ubA[3] = q0_max - q0
ubA[4] = q1_max - q1
ubA[5] = a0_const * Opt[0] + a0_max
ubA[6] = a1_const * Opt[1] + a1_max
return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: "
return -1
old_error1 = error1
old_error2 = error2
old_error3 = error3
example.getPrimalSolution(Opt)
# Update limits
q0 += Opt[0] / 100
q1 += Opt[1] / 100
q_eef = l1 + l2 + l3 + q0 + q1
error1 = p * (q_des1 - q_eef)
error2 = p * (q_des2 - q_eef)
error3 = p * (q_des3 - q_eef)
# Update weights
min_error = min(abs(error1), abs(error2), abs(error3))
max_error = max(abs(error1), abs(error2), abs(error3))
w3 = (1 - (abs(error1) - min_error) /
(max_error - min_error))**3 # goal 1
w4 = (1 - (abs(error2) - min_error) /
(max_error - min_error))**3 # goal 2
w5 = (1 - (abs(error3) - min_error) /
(max_error - min_error))**3 # goal 3
H = np.array([
w1, 0.0, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, 0.0, w3,
0.0, 0.0, 0.0, 0.0, 0.0, w4, 0.0, 0.0, 0.0, 0.0, 0.0, w5
]).reshape((5, 5))
# Stopping conditions
limit1 = abs(error1)
limit2 = abs(error2)
limit3 = abs(error3)
lim = min([limit1, limit2, limit3])
diff1 = abs(error1 - old_error1)
diff2 = abs(error2 - old_error2)
diff3 = abs(error3 - old_error3)
diff = min([diff1, diff2, diff3])
# print 'weights= [ %g, %g, %g ]'%(w3, w4, w5)
# print 'vel = [ %g, %g ], error = [ %g, %g, %g ] \n'% (Opt[0], Opt[1], error1, error2, error3)
# Plotting arrays
pos_eef = np.hstack((pos_eef, q_eef))
pos_0 = np.hstack((pos_0, q0))
pos_1 = np.hstack((pos_1, q1))
vel_0 = np.hstack((vel_0, Opt[0]))
vel_1 = np.hstack((vel_1, Opt[1]))
vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1]))
p_error = np.hstack((p_error, error1))
go_1 = np.hstack((go_1, q_des1))
go_2 = np.hstack((go_2, q_des2))
go_3 = np.hstack((go_3, q_des3))
t = np.hstack((t, i))
# Plot
t_eef = go.Scatter(y=pos_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_eef',
line=dict(dash='dash'))
t_p0 = go.Scatter(y=pos_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_0',
line=dict(dash='dash'))
t_p1 = go.Scatter(y=pos_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_1',
line=dict(dash='dash'))
t_v0 = go.Scatter(y=vel_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_0')
t_v1 = go.Scatter(y=vel_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_1')
t_er = go.Scatter(y=p_error,
x=t,
marker=dict(size=4, ),
mode='lines',
name='error')
t_veef = go.Scatter(y=vel_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_eef')
t_g1 = go.Scatter(y=go_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_1',
line=dict(dash='dot'))
t_g2 = go.Scatter(y=go_2,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_2',
line=dict(dash='dot', width=4))
t_g3 = go.Scatter(y=go_3,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_3',
line=dict(dash='dot', width=6))
data = [t_eef, t_veef, t_g1, t_g2, t_g3]
layout = dict(
title=
"Initial position eef = %g. Goals: goal_1 = %g, goal_2 = %g, goal_3 = %g\n"
% (q_eef_init, q_des1, q_des2, q_des3),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=2,
),
yaxis=dict(title='Position / Velocity'),
font=dict(size=18),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/dynamic_3_weights_EEF.html',
image='png',
image_filename='dyn_3_eef_2')
data = [t_p0, t_v0]
layout = dict(
title="Initial position q0 =%g.\n" % (q0_init),
xaxis=dict(title='Iterations',
autotick=False,
dtick=25,
gridwidth=2),
yaxis=dict(
title='Position / Velocity',
range=[-0.11, .045],
),
font=dict(size=18),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig, filename='html/dynamic_3_weights_q0.html')
data = [t_p1, t_v1]
layout = dict(
title="Initial position q1 = %g.\n" % (q1_init),
xaxis=dict(title='Iterations',
autotick=False,
dtick=25,
gridwidth=2),
yaxis=dict(
title='Position / Velocity',
range=[-0.11, .045],
),
font=dict(size=18),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig, filename='html/dynamic_3_weights__q1.html')
print "\n i = %g \n" % i
return 0
def main():
rospy.init_node('controller_2dof', anonymous=True)
rospy.Rate(10)
# rospy.sleep(0.2)
# Setup data of QP.
# Weights.
w1 = 1e-3
w2 = 1e-3
w3 = 1
w4 = 1 # joint goal
# Joint limits.
q0_max = 0.05
q1_max = 0.15
# Links
l1 = 0.1
l2 = 0.2
l3 = 0.3
# Initial joint values.
q0_init = q0 = 0.02
q1_init = q1 = -0.15
# Joint target.
q_des = 0.6
q0_des = -0.025
q1_des = 0.05
q0_goal = True
q1_goal = False
# Slack limits.
e_max = 1000
e0_max = 1000
# Velocity limits.(+-)
v0_max = 0.05
v1_max = 0.1
# Acceleration limits.(+-)
a0_max = 0.05 * 0.5
a1_max = 0.1 * 0.5
# Others
precision = 5e-3
joint_precision = 5e-3
p = 10
pj = 5
q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1
error = p * (q_des - q_eef)
vel_init = 0
nWSR = np.array([100])
# Acceleration
a0_const = (v0_max - a0_max) / v0_max
a1_const = (v1_max - a1_max) / v1_max
example = SQProblem(4, 7)
H = np.array([
w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0,
w4
]).reshape((4, 4))
A = np.array([
1.0, 1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]).reshape((7, 4))
g = np.array([0.0, 0.0, 0.0, 0.0])
lb = np.array([-v0_max, -v1_max, -e_max, -e0_max])
ub = np.array([v0_max, v1_max, e_max, e0_max])
lbA = np.array(
[error, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max, 0, 0])
ubA = np.array([error, (q0_max - q0), (q1_max - q0), a0_max, a1_max, 0, 0])
# Setting up QProblem object.
if q0_goal is False and q1_goal is False:
print("\nNo joint goal specified\n")
return -1
elif (q0_goal is True and q1_goal is False) or (q0_goal is False
and q1_goal is True):
if q0_goal is True:
lbA[5] = (q0_des - q0)
ubA[5] = (q0_des - q0)
# A[0, 0] = 0.0
A[5, 0] = 1.0
A[5, 3] = 1.0
else:
lbA[5] = (q1_des - q1)
ubA[5] = (q1_des - q1)
A[5, 1] = 1.0
A[5, 3] = 1.0
else:
A[0, 0] = 0.0
A[0, 1] = 0.0
A[0, 2] = 0.0
A[5, 0] = 1.0
A[5, 2] = 1.0
A[6, 1] = 1.0
A[6, 3] = 1.0
p = 0
error = 0
lbA[0] = 0
ubA[0] = 0
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.LOW
example.setOptions(options)
print("Init pos = %g, goal = %g, error = %g, q0 =%g, q1 = %g\n" %
(q_eef, q_des, error, q0, q1))
print A
i = 0
limit = abs(error)
ok = False
Opt = np.zeros(4)
# Plotting
t = np.array(i)
pos_eef = np.array(q_eef)
pos_0 = np.array(q0)
pos_1 = np.array(q1)
vel_0 = np.array(vel_init)
vel_1 = np.array(vel_init)
vel_eef = np.array(vel_init)
p_error = np.array(error)
eef_goal = np.array(q_des)
q0_set = np.array(q0_des)
q1_set = np.array(q1_des)
return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Init of QP-Problem returned without success! ERROR MESSAGE: "
return -1
while not rospy.is_shutdown():
while (limit > precision or not ok) and i < 2000:
# Solve QP.
i += 1
nWSR = np.array([100])
lbA[0] = error
lbA[1] = -q0_max - q0
lbA[2] = -q1_max - q1
lbA[3] = a0_const * Opt[0] - a0_max
lbA[4] = a1_const * Opt[1] - a1_max
ubA[0] = error
ubA[1] = q0_max - q0
ubA[2] = q1_max - q1
ubA[3] = a0_const * Opt[0] + a0_max
ubA[4] = a1_const * Opt[1] + a1_max
return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
rospy.logerr(
"Hotstart of QP-Problem returned without success!")
return -1
# Get and print solution of QP.
example.getPrimalSolution(Opt)
q0 += Opt[0] / 100
q1 += Opt[1] / 100
q_eef = l1 + l2 + l3 + q0 + q1
error = p * (q_des - q_eef)
limit = abs(error)
# print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, error= %g, q0= %g q1= %g \n" % (
# Opt[0], Opt[1], Opt[2], Opt[3], q_eef, error, q0, q1)
# Depending on joint goals
if q0_goal is False and q1_goal is False:
ok = True
error = 0
limit = 0
elif q0_goal is True and q1_goal is False:
lbA[5] = pj * (q0_des - q0)
ubA[5] = pj * (q0_des - q0)
if abs(lbA[5]) < joint_precision:
ok = True
# print "\n q0_error = %g, i = %g \n" % (lbA[5], i)
else:
ok = False
elif q0_goal is False and q1_goal is True:
lbA[5] = pj * (q1_des - q1)
ubA[5] = pj * (q1_des - q1)
if abs(lbA[5]) < joint_precision:
ok = True
# print "\n q0_error = %g, i = %g \n" % (lbA[5], i)
else:
lbA[5] = pj * (q0_des - q0)
ubA[5] = pj * (q0_des - q0)
lbA[6] = pj * (q1_des - q1)
ubA[6] = pj * (q1_des - q1)
if abs(lbA[5]) < joint_precision and abs(
lbA[6]) < joint_precision:
ok = True
# print "\n q0_error = %g, q1_error = %g \n" % (lbA[5], lbA[6])
# Plotting arrays
pos_eef = np.hstack((pos_eef, q_eef))
pos_0 = np.hstack((pos_0, q0))
pos_1 = np.hstack((pos_1, q1))
vel_0 = np.hstack((vel_0, Opt[0]))
vel_1 = np.hstack((vel_1, Opt[1]))
vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1]))
p_error = np.hstack((p_error, error))
eef_goal = np.hstack((eef_goal, q_des))
q0_set = np.hstack((q0_set, q0_des))
q1_set = np.hstack((q1_set, q1_des))
t = np.hstack((t, i))
# Plot
t_eef = go.Scatter(y=pos_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_eef',
line=dict(dash='dash'))
t_p0 = go.Scatter(y=pos_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_q0',
line=dict(dash='dash'))
t_p1 = go.Scatter(y=pos_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_q1',
line=dict(dash='dash'))
t_v0 = go.Scatter(y=vel_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_q0')
t_v1 = go.Scatter(y=vel_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_q1')
t_veef = go.Scatter(y=vel_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_eef')
t_er = go.Scatter(y=p_error,
x=t,
marker=dict(size=4, ),
mode='lines',
name='error')
t_g1 = go.Scatter(y=q0_set,
x=t,
marker=dict(size=4, ),
mode='lines',
name='joint0_goal',
line=dict(dash='dot'))
t_g2 = go.Scatter(y=q1_set,
x=t,
marker=dict(size=4, ),
mode='lines',
name='joint1_goal',
line=dict(dash='dot'))
t_goal = go.Scatter(y=eef_goal,
x=t,
marker=dict(size=4, ),
mode='lines',
name='eef_goal',
line=dict(dash='dot'))
if q0_goal == True:
data_q0 = [t_p0, t_v0, t_g1]
data_q1 = [t_p1, t_v1]
else:
data_q0 = [t_p0, t_v0]
data_q1 = [t_p1, t_v1, t_g2]
layout = dict(title="Initial position q0 =%g.\n" % (q0_init),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
tickcolor='#060',
),
yaxis=dict(title='Position / Velocity',
gridwidth=3,
range=[-0.15, .1]),
font=dict(size=18))
fig0 = dict(data=data_q0, layout=layout)
plotly.offline.plot(fig0,
filename='joint_goals_q0.html',
image='png',
image_filename='joint_q0')
layout = dict(title="Initial position q1 = %g.\n" % (q1_init),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
tickcolor='#060',
),
yaxis=dict(
title='Position / Velocity',
gridwidth=3,
),
font=dict(size=18))
fig1 = dict(data=data_q1, layout=layout)
plotly.offline.plot(fig1,
filename='joint_goals_q1.html',
image='png',
image_filename='joint_q1')
data_eef = [t_eef, t_veef, t_goal]
layout_eef = dict(title="Initial position EEF = %g. Goal = %g, \n" %
(q_eef_init, q_des),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
),
yaxis=dict(title='Position / Velocity', gridwidth=3),
font=dict(size=16))
fig = dict(data=data_eef, layout=layout_eef)
plotly.offline.plot(fig,
filename='joint_goals_eef.html',
image='png',
image_filename='joint_eef')
print "\n i = ", i
return 0
print ok
def __init__(
self, N=16, T=0.1, T_step=0.8,
fsm_state='D', fsm_sl=1
):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(ClassicGenerator, self).__init__(
N, T, T_step, fsm_state, fsm_sl
)
# TODO for speed up one can define members of BaseGenerator as
# direct views of QP data structures according to walking report
# Maybe do that later!
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x^T * H(w0) * x + x^T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # number of working set recalculations
self.options = Options()
self.options.setToMPC()
self.options.printLevel = PrintLevel.LOW
# FOR ORIENTATIONS
# define dimensions
self.ori_nv = 2*self.N
self.ori_nc = (
self.nc_fvel_eq
+ self.nc_fpos_ineq
+ self.nc_fvel_ineq
)
# setup problem
self.ori_dofs = numpy.zeros(self.ori_nv)
self.ori_qp = SQProblem(self.ori_nv, self.ori_nc)
self.ori_qp.setOptions(self.options) # load NMPC options
self.ori_H = numpy.zeros((self.ori_nv,self.ori_nv))
self.ori_A = numpy.zeros((self.ori_nc,self.ori_nv))
self.ori_g = numpy.zeros((self.ori_nv,))
self.ori_lb = -numpy.ones((self.ori_nv,))*1e+08
self.ori_ub = numpy.ones((self.ori_nv,))*1e+08
self.ori_lbA = -numpy.ones((self.ori_nc,))*1e+08
self.ori_ubA = numpy.ones((self.ori_nc,))*1e+08
self._ori_qp_is_initialized = False
# save computation time and working set recalculations
self.ori_qp_nwsr = 0.0
self.ori_qp_cputime = 0.0
# FOR POSITIONS
# define dimensions
self.pos_nv = 2*(self.N + self.nf)
self.pos_nc = (
self.nc_cop
+ self.nc_foot_position
+ self.nc_fchange_eq
)
# setup problem
self.pos_dofs = numpy.zeros(self.pos_nv)
self.pos_qp = SQProblem(self.pos_nv, self.pos_nc)
self.pos_qp.setOptions(self.options)
self.pos_H = numpy.zeros((self.pos_nv,self.pos_nv))
self.pos_A = numpy.zeros((self.pos_nc,self.pos_nv))
self.pos_g = numpy.zeros((self.pos_nv,))
self.pos_lb = -numpy.ones((self.pos_nv,))*1e+08
self.pos_ub = numpy.ones((self.pos_nv,))*1e+08
self.pos_lbA = -numpy.ones((self.pos_nc,))*1e+08
self.pos_ubA = numpy.ones((self.pos_nc,))*1e+08
self._pos_qp_is_initialized = False
# save computation time and working set recalculations
self.pos_qp_nwsr = 0.0
self.pos_qp_cputime = 0.0
# dummy matrices
self._ori_Q = numpy.zeros((2*self.N, 2*self.N))
self._ori_p = numpy.zeros((2*self.N,))
self._pos_Q = numpy.zeros((self.N + self.nf, self.N + self.nf))
self._pos_p = numpy.zeros((self.N + self.nf,))
# add additional keys that should be saved
self._data_keys.append('ori_qp_nwsr')
self._data_keys.append('ori_qp_cputime')
self._data_keys.append('pos_qp_nwsr')
self._data_keys.append('pos_qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
def __init__(
self, N=16, T=0.1, T_step=0.8,
fsm_state='D', fsm_sl=1
):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(NMPCGenerator, self).__init__(
N, T, T_step, fsm_state, fsm_sl
)
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x.T * H(w0) * x + x.T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # # of working set recalculations
self.options = Options()
self.options.setToMPC()
#self.options.printLevel = PrintLevel.LOW
# define variable dimensions
# variables of: position + orientation
self.nv = 2*(self.N+self.nf) + 2*N
# define constraint dimensions
self.nc_pos = (
self.nc_cop
+ self.nc_foot_position
+ self.nc_fchange_eq
)
self.nc_ori = (
self.nc_fvel_eq
+ self.nc_fpos_ineq
+ self.nc_fvel_ineq
)
self.nc = (
# position
self.nc_pos
# orientation
+ self.nc_ori
)
# setup problem
self.dofs = numpy.zeros(self.nv)
self.qp = SQProblem(self.nv, self.nc)
# load NMPC options
self.qp.setOptions(self.options)
self.qp_H = numpy.eye(self.nv,self.nv)
self.qp_A = numpy.zeros((self.nc,self.nv))
self.qp_g = numpy.zeros((self.nv,))
self.qp_lb = -numpy.ones((self.nv,))*1e+08
self.qp_ub = numpy.ones((self.nv,))*1e+08
self.qp_lbA = -numpy.ones((self.nc,))*1e+08
self.qp_ubA = numpy.ones((self.nc,))*1e+08
self._qp_is_initialized = False
# save computation time and working set recalculations
self.qp_nwsr = 0.0
self.qp_cputime = 0.0
# setup analyzer for solution analysis
analyser = SolutionAnalysis()
# helper matrices for common expressions
self.Hx = numpy.zeros((1, 2*(N+nf)), dtype=float)
self.Q_k_x = numpy.zeros((N+nf, N+nf), dtype=float)
self.p_k_x = numpy.zeros((N+nf,), dtype=float)
self.p_k_y = numpy.zeros((N+nf,), dtype=float)
self.Hq = numpy.zeros((1, 2*N), dtype=float)
self.Q_k_qR = numpy.zeros((N, N), dtype=float)
self.Q_k_qL = numpy.zeros((N, N), dtype=float)
self.p_k_qR = numpy.zeros((N), dtype=float)
self.p_k_qL = numpy.zeros((N), dtype=float)
self.A_pos_x = numpy.zeros((self.nc_pos, 2*(N+nf)), dtype=float)
self.A_pos_q = numpy.zeros((self.nc_pos, 2*N), dtype=float)
self.ubA_pos = numpy.zeros((self.nc_pos,), dtype=float)
self.lbA_pos = numpy.zeros((self.nc_pos,), dtype=float)
self.A_ori = numpy.zeros((self.nc_ori, 2*N), dtype=float)
self.ubA_ori = numpy.zeros((self.nc_ori,), dtype=float)
self.lbA_ori = numpy.zeros((self.nc_ori,), dtype=float)
self.derv_Acop_map = numpy.zeros((self.nc_cop, self.N), dtype=float)
self.derv_Afoot_map = numpy.zeros((self.nc_foot_position, self.N), dtype=float)
self._update_foot_selection_matrix()
# add additional keys that should be saved
self._data_keys.append('qp_nwsr')
self._data_keys.append('qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
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)
H = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
g = np.array([1.5, 1.0 ])
lb = np.array([0.5, -2.0])
ub = np.array([5.0, 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])
# Setting up QProblemB object.
example = QProblemB(2)
options = Options()
options.enableFlippingBounds = BooleanType.FALSE
options.initialStatusBounds = SubjectToStatus.INACTIVE
options.numRefinementSteps = 1
example.setOptions(options)
# Solve first QP.
nWSR = np.array([10])
example.init(H, g, lb, ub, nWSR)
print("\nnWSR = %d\n\n"%nWSR)
# Solve second QP.
nWSR = np.array([10])
example.hotstart(g_new, lb_new, ub_new, nWSR)
print("\nnWSR = %d\n\n"% nWSR)
class NMPCGenerator(BaseGenerator):
"""
Implementation of the combined problems using NMPC techniques.
Solve QP for position and orientation of CoM and feet simultaneously in
each timestep. Calculates derivatives and updates states in each step.
"""
def __init__(
self, N=16, T=0.1, T_step=0.8,
fsm_state='D', fsm_sl=1
):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(NMPCGenerator, self).__init__(
N, T, T_step, fsm_state, fsm_sl
)
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x.T * H(w0) * x + x.T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # # of working set recalculations
self.options = Options()
self.options.setToMPC()
#self.options.printLevel = PrintLevel.LOW
# define variable dimensions
# variables of: position + orientation
self.nv = 2*(self.N+self.nf) + 2*N
# define constraint dimensions
self.nc_pos = (
self.nc_cop
+ self.nc_foot_position
+ self.nc_fchange_eq
)
self.nc_ori = (
self.nc_fvel_eq
+ self.nc_fpos_ineq
+ self.nc_fvel_ineq
)
self.nc = (
# position
self.nc_pos
# orientation
+ self.nc_ori
)
# setup problem
self.dofs = numpy.zeros(self.nv)
self.qp = SQProblem(self.nv, self.nc)
# load NMPC options
self.qp.setOptions(self.options)
self.qp_H = numpy.eye(self.nv,self.nv)
self.qp_A = numpy.zeros((self.nc,self.nv))
self.qp_g = numpy.zeros((self.nv,))
self.qp_lb = -numpy.ones((self.nv,))*1e+08
self.qp_ub = numpy.ones((self.nv,))*1e+08
self.qp_lbA = -numpy.ones((self.nc,))*1e+08
self.qp_ubA = numpy.ones((self.nc,))*1e+08
self._qp_is_initialized = False
# save computation time and working set recalculations
self.qp_nwsr = 0.0
self.qp_cputime = 0.0
# setup analyzer for solution analysis
analyser = SolutionAnalysis()
# helper matrices for common expressions
self.Hx = numpy.zeros((1, 2*(N+nf)), dtype=float)
self.Q_k_x = numpy.zeros((N+nf, N+nf), dtype=float)
self.p_k_x = numpy.zeros((N+nf,), dtype=float)
self.p_k_y = numpy.zeros((N+nf,), dtype=float)
self.Hq = numpy.zeros((1, 2*N), dtype=float)
self.Q_k_qR = numpy.zeros((N, N), dtype=float)
self.Q_k_qL = numpy.zeros((N, N), dtype=float)
self.p_k_qR = numpy.zeros((N), dtype=float)
self.p_k_qL = numpy.zeros((N), dtype=float)
self.A_pos_x = numpy.zeros((self.nc_pos, 2*(N+nf)), dtype=float)
self.A_pos_q = numpy.zeros((self.nc_pos, 2*N), dtype=float)
self.ubA_pos = numpy.zeros((self.nc_pos,), dtype=float)
self.lbA_pos = numpy.zeros((self.nc_pos,), dtype=float)
self.A_ori = numpy.zeros((self.nc_ori, 2*N), dtype=float)
self.ubA_ori = numpy.zeros((self.nc_ori,), dtype=float)
self.lbA_ori = numpy.zeros((self.nc_ori,), dtype=float)
self.derv_Acop_map = numpy.zeros((self.nc_cop, self.N), dtype=float)
self.derv_Afoot_map = numpy.zeros((self.nc_foot_position, self.N), dtype=float)
self._update_foot_selection_matrix()
# add additional keys that should be saved
self._data_keys.append('qp_nwsr')
self._data_keys.append('qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
def solve(self):
""" Process and solve problem, s.t. pattern generator data is consistent """
self._preprocess_solution()
self._solve_qp()
self._postprocess_solution()
def _preprocess_solution(self):
""" Update matrices and get them into the QP data structures """
# rename for convenience
N = self.N
nf = self.nf
# dofs
dofs = self.dofs
dddC_k_x = self.dddC_k_x
F_k_x = self.F_k_x
dddC_k_y = self.dddC_k_y
F_k_y = self.F_k_y
dddF_k_qR = self.dddF_k_qR
dddF_k_qL = self.dddF_k_qL
# inject dofs for convenience
# dofs = ( dddC_k_x ) N
# ( F_k_x ) nf
# ( dddC_k_y ) N
# ( F_k_y ) nf
# ( dddF_k_q ) N
# ( dddF_k_q ) N
dofs[0 :0+N ] = dddC_k_x
dofs[0+N:0+N+nf] = F_k_x
a = N+nf
dofs[a :a+N ] = dddC_k_y
dofs[a+N:a+N+nf] = F_k_y
a = 2*(N+nf)
dofs[ a:a+N] = dddF_k_qR
dofs[ -N:] = dddF_k_qL
# define position and orientation dofs
# U_k = ( U_k_xy, U_k_q).T
# U_k_xy = ( dddC_k_x ) N
# ( F_k_x ) nf
# ( dddC_k_y ) N
# ( F_k_y ) nf
# U_k_q = ( dddF_k_q ) N
# ( dddF_k_q ) N
# position dofs
U_k = self.dofs
U_k_xy = U_k[ :2*(N+nf)]
U_k_x = U_k_xy[:(N+nf)]
U_k_y = U_k_xy[(N+nf):]
# orientation dofs
U_k_q = U_k [-2*N: ]
U_k_qR = U_k_q[ :N]
U_k_qL = U_k_q[ N: ]
# position dimensions
nU_k = U_k.shape[0]
nU_k_xy = U_k_xy.shape[0]
nU_k_x = U_k_x.shape[0]
nU_k_y = U_k_y.shape[0]
# orientation dimensions
nU_k_q = U_k_q.shape[0]
nU_k_qR = U_k_qR.shape[0]
nU_k_qL = U_k_qL.shape[0]
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._qp_is_initialized:
# TODO guess initial active set
# this requires changes to the python interface
pass
# calculate some common sub expressions
self._calculate_common_expressions()
# calculate Jacobian parts that are non-trivial, i.e. wrt. to orientation
self._calculate_derivatives()
# POSITION QP
# rename matrices
Q_k_x = self.Q_k_x
Q_k_y = self.Q_k_x # NOTE it's exactly the same!
p_k_x = self.p_k_x
p_k_y = self.p_k_y
# ORIENTATION QP
# rename matrices
Q_k_qR = self.Q_k_qR
Q_k_qL = self.Q_k_qL
p_k_qR = self.p_k_qR
p_k_qL = self.p_k_qL
# define QP matrices
# Gauss-Newton Hessian approximation
# define sub blocks
# H = (Hxy | Hqx)
# (Hxq | Hqq)
Hxx = self.qp_H[:nU_k_xy,:nU_k_xy]
Hxq = self.qp_H[:nU_k_xy,nU_k_xy:]
Hqx = self.qp_H[nU_k_xy:,:nU_k_xy]
Hqq = self.qp_H[nU_k_xy:,nU_k_xy:]
# fill matrices
Hxx[ :nU_k_x, :nU_k_x] = Q_k_x
Hxx[-nU_k_y:,-nU_k_y:] = Q_k_y
Hqq[ :nU_k_qR, :nU_k_qR] = Q_k_qR
Hqq[-nU_k_qL:,-nU_k_qL:] = Q_k_qL
#self.qp_H[...] = numpy.eye(self.nv)
# Gradient of Objective
# define sub blocks
# g = (gx)
# (gq)
gx = self.qp_g[:nU_k_xy]
gq = self.qp_g[-nU_k_q:]
# gx = ( U_k_x.T Q_k_x + p_k_x )
gx[ :nU_k_x] = U_k_x.dot(Q_k_x) + p_k_x
gx[-nU_k_y:] = U_k_y.dot(Q_k_y) + p_k_y
# gq = ( U_k_q.T Q_k_q + p_k_q )
gq[ :nU_k_qR] = U_k_qR.dot(Q_k_qR) + p_k_qR
gq[-nU_k_qL:] = U_k_qL.dot(Q_k_qL) + p_k_qL
# CONSTRAINTS
# A = ( A_xy, A_xyq )
# ( 0, A_q )
A_xy = self.qp_A [:self.nc_pos,:nU_k_xy]
A_xyq = self.qp_A [:self.nc_pos,-nU_k_q:]
lbA_xy = self.qp_lbA[:self.nc_pos]
ubA_xy = self.qp_ubA[:self.nc_pos]
A_q = self.qp_A [-self.nc_ori:,-nU_k_q:]
lbA_q = self.qp_lbA[-self.nc_ori:]
ubA_q = self.qp_ubA[-self.nc_ori:]
# linearized constraints are given by
# lbA - A * U_k <= nablaA * Delta_U_k <= ubA - A * U_k
A_xy[...] = self.A_pos_x
A_xyq[...] = self.A_pos_q
lbA_xy[...] = self.lbA_pos - self.A_pos_x.dot(U_k_xy)
ubA_xy[...] = self.ubA_pos - self.A_pos_x.dot(U_k_xy)
A_q[...] = self.A_ori
lbA_q[...] = self.lbA_ori - self.A_ori.dot(U_k_q)
ubA_q[...] = self.ubA_ori - self.A_ori.dot(U_k_q)
def _calculate_common_expressions(self):
"""
encapsulation of complicated matrix assembly of former orientation and
position QP sub matrices
"""
#rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
Pvs = self.Pvs
Pvu = self.Pvu
Pzs = self.Pzs
Pzu = self.Pzu
c_k_x = self.c_k_x
c_k_y = self.c_k_y
f_k_x = self.f_k_x
f_k_y = self.f_k_y
f_k_qR = self.f_k_qR
f_k_qL = self.f_k_qL
v_kp1 = self.v_kp1
V_kp1 = self.V_kp1
dC_kp1_x_ref = self.dC_kp1_x_ref
dC_kp1_y_ref = self.dC_kp1_y_ref
dC_kp1_q_ref = self.dC_kp1_q_ref
# POSITION QP MATRICES
# Q_k_x = ( Q_k_xXX Q_k_xXF ) = Q_k_y
# ( Q_k_xFX Q_k_xFF )
Q_k_x = self.Q_k_x
a = 0; b = N
c = 0; d = N
Q_k_xXX = Q_k_x[a:b,c:d]
a = 0; b = N
c = N; d = N+nf
Q_k_xXF = Q_k_x[a:b,c:d]
a = N; b = N+nf
c = 0; d = N
Q_k_xFX = Q_k_x[a:b,c:d]
a = N; b = N+nf
c = N; d = N+nf
Q_k_xFF = Q_k_x[a:b,c:d]
# Q_k_xXX = ( 0.5 * a * Pvu^T * Pvu + c * Pzu^T * Pzu + d * I )
# Q_k_xXF = ( -0.5 * c * Pzu^T * V_kp1 )
# Q_k_xFX = ( -0.5 * c * Pzu^T * V_kp1 )^T
# Q_k_xFF = ( 0.5 * c * V_kp1^T * V_kp1 )
Q_k_xXX[...] = (
alpha * Pvu.transpose().dot(Pvu)
+ gamma * Pzu.transpose().dot(Pzu)
+ delta * numpy.eye(N)
)
Q_k_xXF[...] = - gamma * Pzu.transpose().dot(V_kp1)
Q_k_xFX[...] = Q_k_xXF.transpose()
Q_k_xFF[...] = gamma * V_kp1.transpose().dot(V_kp1)
# p_k_x = ( p_k_xX )
# ( p_k_xF )
p_k_x = self.p_k_x
p_k_xX = p_k_x[ :N]
p_k_xF = p_k_x[-nf: ]
# p_k_xX = ( 0.5 * a * Pvu^T * Pvu + c * Pzu^T * Pzu + d * I )
# p_k_xF = ( -0.5 * c * Pzu^T * V_kp1 )
p_k_xX[...] = alpha * Pvu.transpose().dot( Pvs.dot(c_k_x) - dC_kp1_x_ref) \
+ gamma * Pzu.transpose().dot( Pzs.dot(c_k_x) - v_kp1.dot(f_k_x))
p_k_xF[...] =-gamma * V_kp1.transpose().dot(Pzs.dot(c_k_x) - v_kp1.dot(f_k_x))
# p_k_y = ( p_k_yX )
# ( p_k_yF )
p_k_y = self.p_k_y
p_k_yX = p_k_y[ :N]
p_k_yF = p_k_y[-nf: ]
# p_k_yX = ( 0.5 * a * Pvu^T * Pvu + c * Pzu^T * Pzu + d * I )
# p_k_yF = ( -0.5 * c * Pzu^T * V_kp1 )
p_k_yX[...] = alpha * Pvu.transpose().dot( Pvs.dot(c_k_y) - dC_kp1_y_ref) \
+ gamma * Pzu.transpose().dot( Pzs.dot(c_k_y) - v_kp1.dot(f_k_y))
p_k_yF[...] =-gamma * V_kp1.transpose().dot(Pzs.dot(c_k_y) - v_kp1.dot(f_k_y))
# ORIENTATION QP MATRICES
# Q_k_qR = ( 0.5 * a * Pvu^T * E_FR^T * E_FR * Pvu )
Q_k_qR = self.Q_k_qR
Q_k_qR[...] = alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR).dot(Pvu)
# p_k_qR = ( a * Pvu^T * E_FR^T * (E_FR * Pvs * f_k_qR + dC_kp1_q_ref) )
p_k_qR = self.p_k_qR
p_k_qR[...] = alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR.dot(Pvs).dot(f_k_qR) - dC_kp1_q_ref)
# Q_k_qL = ( 0.5 * a * Pvu^T * E_FL^T * E_FL * Pvu )
Q_k_qL = self.Q_k_qL
Q_k_qL[...] = alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL).dot(Pvu)
# p_k_qL = ( a * Pvu^T * E_FL^T * (E_FL * Pvs * f_k_qL + dC_kp1_q_ref) )
p_k_qL = self.p_k_qL
p_k_qL[...] = alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL.dot(Pvs).dot(f_k_qL) - dC_kp1_q_ref)
# LINEAR CONSTRAINTS
# CoP constraints
a = 0
b = self.nc_cop
self.A_pos_x[a:b] = self.Acop
self.lbA_pos[a:b] = self.lbBcop
self.ubA_pos[a:b] = self.ubBcop
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.A_pos_x[a:b] = self.Afoot
self.lbA_pos[a:b] = self.lbBfoot
self.ubA_pos[a:b] = self.ubBfoot
#foot equality constraints
a = self.nc_cop + self.nc_foot_position
b = self.nc_cop + self.nc_foot_position + self.nc_fchange_eq
self.A_pos_x[a:b] = self.eqAfoot
self.lbA_pos[a:b] = self.eqBfoot
self.ubA_pos[a:b] = self.eqBfoot
# velocity constraints on support foot to freeze movement
a = 0
b = self.nc_fvel_eq
self.A_ori [a:b] = self.A_fvel_eq
self.lbA_ori[a:b] = self.B_fvel_eq
self.ubA_ori[a:b] = self.B_fvel_eq
# box constraints for maximum orientation change
a = self.nc_fvel_eq
b = self.nc_fvel_eq + self.nc_fpos_ineq
self.A_ori [a:b] = self.A_fpos_ineq
self.lbA_ori[a:b] = self.lbB_fpos_ineq
self.ubA_ori[a:b] = self.ubB_fpos_ineq
# box constraints for maximum angular velocity
a = self.nc_fvel_eq + self.nc_fpos_ineq
b = self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq
self.A_ori [a:b] = self.A_fvel_ineq
self.lbA_ori[a:b] = self.lbB_fvel_ineq
self.ubA_ori[a:b] = self.ubB_fvel_ineq
def _calculate_derivatives(self):
""" calculate the Jacobian of the constraints function """
# COP CONSTRAINTS
# build the constraint enforcing the center of pressure to stay inside
# the support polygon given through the convex hull of the foot.
# define dummy values
# D_kp1 = (D_kp1x, Dkp1_y)
D_kp1 = numpy.zeros( (self.nFootEdge*self.N, 2*self.N), dtype=float )
D_kp1x = D_kp1[:, :self.N] # view on big matrix
D_kp1y = D_kp1[:,-self.N:] # view on big matrix
b_kp1 = numpy.zeros( (self.nFootEdge*self.N,), dtype=float )
# change entries according to support order changes in D_kp1
theta_vec = [self.f_k_q,self.F_k_q[0],self.F_k_q[1]]
for i in range(self.N):
theta = theta_vec[self.supportDeque[i].stepNumber]
# NOTE THIS CHANGES DUE TO APPLYING THE DERIVATIVE!
rotMat = numpy.array([
# old
# [ cos(theta), sin(theta)],
# [-sin(theta), cos(theta)]
# new: derivative wrt to theta
[-numpy.sin(theta), numpy.cos(theta)],
[-numpy.cos(theta),-numpy.sin(theta)]
])
if self.supportDeque[i].foot == "left" :
A0 = self.A0lf.dot(rotMat)
B0 = self.ubB0lf
D0 = self.A0dlf.dot(rotMat)
d0 = self.ubB0dlf
else :
A0 = self.A0rf.dot(rotMat)
B0 = self.ubB0rf
D0 = self.A0drf.dot(rotMat)
d0 = self.ubB0drf
# get support foot and check if it is double support
for j in range(self.nf):
if self.V_kp1[i,j] == 1:
if self.fsm_states[j] == 'D':
A0 = D0
B0 = d0
else:
pass
for k in range(self.nFootEdge):
# get d_i+1^x(f^theta)
D_kp1x[i*self.nFootEdge+k, i] = A0[k][0]
# get d_i+1^y(f^theta)
D_kp1y[i*self.nFootEdge+k, i] = A0[k][1]
# get right hand side of equation
b_kp1 [i*self.nFootEdge+k] = B0[k]
#rename for convenience
N = self.N
nf = self.nf
PzuV = self.PzuV
PzuVx = self.PzuVx
PzuVy = self.PzuVy
PzsC = self.PzsC
PzsCx = self.PzsCx
PzsCy = self.PzsCy
v_kp1fc = self.v_kp1fc
v_kp1fc_x = self.v_kp1fc_x
v_kp1fc_y = self.v_kp1fc_y
# build constraint transformation matrices
# PzuV = ( PzuVx )
# ( PzuVy )
# PzuVx = ( Pzu | -V_kp1 | 0 | 0 )
PzuVx[:, :self.N ] = self.Pzu # TODO this is constant in matrix and should go into the build up matrice part
PzuVx[:,self.N:self.N+self.nf] = -self.V_kp1
# PzuVy = ( 0 | 0 | Pzu | -V_kp1 )
PzuVy[:,-self.N-self.nf:-self.nf] = self.Pzu # TODO this is constant in matrix and should go into the build up matrice part
PzuVy[:, -self.nf: ] = -self.V_kp1
# PzuV = ( PzsCx ) = ( Pzs * c_k_x)
# ( PzsCy ) ( Pzs * c_k_y)
PzsCx[...] = self.Pzs.dot(self.c_k_x) #+ self.v_kp1.dot(self.f_k_x)
PzsCy[...] = self.Pzs.dot(self.c_k_y) #+ self.v_kp1.dot(self.f_k_y)
# v_kp1fc = ( v_kp1fc_x ) = ( v_kp1 * f_k_x)
# ( v_kp1fc_y ) ( v_kp1 * f_k_y)
v_kp1fc_x[...] = self.v_kp1.dot(self.f_k_x)
v_kp1fc_y[...] = self.v_kp1.dot(self.f_k_y)
# build CoP linear constraints
# NOTE D_kp1 is member and D_kp1 = ( D_kp1x | D_kp1y )
# D_kp1x,y contains entries from support polygon
dummy = D_kp1.dot(PzuV)
dummy = dummy.dot(self.dofs[:2*(N+nf)])
# CoP constraints
a = 0
b = self.nc_cop
self.A_pos_q[a:b, :N] = \
dummy.dot(self.derv_Acop_map).dot(self.E_FR_bar).dot(self.Ppu)
self.A_pos_q[a:b,-N:] = \
dummy.dot(self.derv_Acop_map).dot(self.E_FL_bar).dot(self.Ppu)
# FOOT POSITION CONSTRAINTS
# defined on the horizon
# inequality constraint on both feet A u + B <= 0
# A0 R(theta) [Fx_k+1 - Fx_k] <= ubB0
# [Fy_k+1 - Fy_k]
matSelec = numpy.array([ [1, 0],[-1, 1] ])
footSelec = numpy.array([ [self.f_k_x, 0],[self.f_k_y, 0] ])
theta_vec = [self.f_k_q, self.F_k_q[0]]
# rotation matrice from F_k+1 to F_k
# NOTE THIS CHANGES DUE TO APPLYING THE DERIVATIVE!
rotMat1 = numpy.array([
# old
# [cos(theta_vec[0]), sin(theta_vec[0])],
# [-sin(theta_vec[0]), cos(theta_vec[0])]
# new: derivative wrt to theta
[-numpy.sin(theta_vec[0]), numpy.cos(theta_vec[0])],
[-numpy.cos(theta_vec[0]),-numpy.sin(theta_vec[0])]
])
rotMat2 = numpy.array([
# old
# [cos(theta_vec[1]), sin(theta_vec[1])],
# [-sin(theta_vec[1]), cos(theta_vec[1])]
# new
[-numpy.sin(theta_vec[1]), numpy.cos(theta_vec[1])],
[-numpy.cos(theta_vec[1]),-numpy.sin(theta_vec[1])]
])
nf = self.nf
nEdges = self.A0l.shape[0]
N = self.N
ncfoot = nf * nEdges
if self.currentSupport.foot == "left":
A_f1 = self.A0r.dot(rotMat1)
A_f2 = self.A0l.dot(rotMat2)
else :
A_f1 = self.A0l.dot(rotMat1)
A_f2 = self.A0r.dot(rotMat2)
tmp1 = numpy.array( [A_f1[:,0],numpy.zeros((nEdges,),dtype=float)] )
tmp2 = numpy.array( [numpy.zeros((nEdges,),dtype=float),A_f2[:,0]] )
tmp3 = numpy.array( [A_f1[:,1],numpy.zeros((nEdges,),dtype=float)] )
tmp4 = numpy.array( [numpy.zeros(nEdges,),A_f2[:,1]] )
X_mat = numpy.concatenate( (tmp1.T,tmp2.T) , 0)
A0x = X_mat.dot(matSelec)
Y_mat = numpy.concatenate( (tmp3.T,tmp4.T) , 0)
A0y = Y_mat.dot(matSelec)
dummy = numpy.concatenate ((
numpy.zeros((ncfoot,N),dtype=float), A0x,
numpy.zeros((ncfoot,N),dtype=float), A0y
), 1
)
dummy = dummy.dot(self.dofs[:2*(N+nf)])
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.A_pos_q[a:b, :N] = dummy.dot(self.derv_Afoot_map).dot(self.E_FR_bar).dot(self.Ppu)
self.A_pos_q[a:b,-N:] = dummy.dot(self.derv_Afoot_map).dot(self.E_FL_bar).dot(self.Ppu)
def _solve_qp(self):
"""
Solve QP first run with init functionality and other runs with warmstart
"""
self.cpu_time = 2.9 # ms
self.nwsr = 1000 # unlimited bounded
if not self._qp_is_initialized:
ret, nwsr, cputime = self.qp.init(
self.qp_H, self.qp_g, self.qp_A,
self.qp_lb, self.qp_ub,
self.qp_lbA, self.qp_ubA,
self.nwsr, self.cpu_time
)
self._qp_is_initialized = True
else:
ret, nwsr, cputime = self.qp.hotstart(
self.qp_H, self.qp_g, self.qp_A,
self.qp_lb, self.qp_ub,
self.qp_lbA, self.qp_ubA,
self.nwsr, self.cpu_time
)
# orientation primal solution
self.qp.getPrimalSolution(self.dofs)
# save qp solver data
self.qp_nwsr = nwsr # working set recalculations
self.qp_cputime = cputime*1000. # in milliseconds (set to 2.9ms)
def _postprocess_solution(self):
""" Get solution and put it back into generator data structures """
# rename for convenience
N = self.N
nf = self.nf
# extract dofs
# dofs = ( dddC_k_x ) N
# ( F_k_x ) nf
# ( dddC_k_y ) N
# ( F_k_y ) nf
# ( dddF_k_q ) N
# ( dddF_k_q ) N
# NOTE this time we add an increment to the existing values
# data(k+1) = data(k) + alpha * dofs
# TODO add line search when problematic
alpha = 1.0
# x values
self.dddC_k_x[:] += alpha * self.dofs[0 :0+N ]
self.F_k_x[:] += alpha * self.dofs[0+N:0+N+nf]
# y values
a = N + nf
self.dddC_k_y[:] += alpha * self.dofs[a :a+N ]
self.F_k_y[:] += alpha * self.dofs[a+N:a+N+nf]
# feet orientation
a =2*(N + nf)
self.dddF_k_qR[:] += alpha * self.dofs[ a:a+N]
self.dddF_k_qL[:] += alpha * self.dofs[ -N:]
def update(self):
"""
overload update function to define time dependent support foot selection
matrix.
"""
ret = super(NMPCGenerator, self).update()
# update selection matrix when something has changed
self._update_foot_selection_matrix()
return ret
def _update_foot_selection_matrix(self):
""" get right foot selection matrix """
i = 0
for j in range(self.N):
self.derv_Acop_map[i:i+self.nFootEdge,j] = 1.0
i += self.nFootEdge
self.derv_Afoot_map[...] = 0.0
i = self.nFootPosHullEdges
for j in range(self.nf-1):
for k in range(self.N):
if self.V_kp1[k,j] == 1:
self.derv_Afoot_map[i:i+self.nFootPosHullEdges,k] = 1.0
i += self.nFootPosHullEdges
break
else:
self.derv_Afoot_map[i:i+self.nFootPosHullEdges,j] = 0.0
# A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
# details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with qpsolvers. If not, see <http://www.gnu.org/licenses/>.
from numpy import array, hstack, ones, vstack, zeros
from qpoases import PyOptions as Options
from qpoases import PyPrintLevel as PrintLevel
from qpoases import PyQProblem as QProblem
from qpoases import PyQProblemB as QProblemB
from qpoases import PyReturnValue as ReturnValue
__infty = 1e10
options = Options()
options.printLevel = PrintLevel.NONE
def qpoases_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None,
max_wsr=1000):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
def qpoases_calculation(self, error_posit):
# Variable initialization
self._feedback.sim_trajectory = [self.current_state]
error_orient = np.array([0.0, 0.0, 0.0])
eef_pose_array = PoseArray()
reached_orientation = False
self.trajectory_length = 0.0
self.old_dist = 0.0
# Setting up QProblem object
vel_calculation = SQProblem(self.problem_size[1], self.problem_size[0])
options = Options()
options.setToDefault()
options.printLevel = PrintLevel.LOW
vel_calculation.setOptions(options)
Opt = np.zeros(self.problem_size[1])
i = 0
# Config iai_naive_kinematics_sim, send commands
clock = ProjectionClock()
velocity_msg = JointState()
velocity_msg.name = self.all_joint_names
velocity_msg.header.stamp = rospy.get_rostime()
velocity_array = [0.0 for x in range(len(self.all_joint_names))]
effort_array = [0.0 for x in range(len(self.all_joint_names))]
velocity_msg.velocity = velocity_array
velocity_msg.effort = effort_array
velocity_msg.position = effort_array
clock.now = rospy.get_rostime()
clock.period.nsecs = 1000000
self.pub_clock.publish(clock)
# Plot for debugging
'''t = np.array([i])
base_weight = np.array(self.jweights[0, 0])
arm_weight = np.array(self.jweights[4, 4])
triang_weight = np.array(self.jweights[3, 3])
low, pos, high, vel_p, error = [], [], [], [], []
error = np.array([0])
for x in range(8+3):
low.append(np.array([self.joint_limits_lower[x]]))
pos.append(np.array(self.joint_values[x]))
vel_p.append(np.array(Opt[x]))
high.append(np.array([self.joint_limits_upper[x]]))#'''
tic = rospy.get_rostime()
while not self.reach_position or not reached_orientation or not self.reach_pregrasp:
i += 1
# Check if client cancelled goal
if not self.active_goal_flag:
self.success = False
break
if i > 500:
self.abort_goal('time_out')
break
# Solve QP, redefine limit vectors of A.
eef_pose_msg = Pose()
nWSR = np.array([100])
[ac_lim_lower, ac_lim_upper] = self.acceleration_limits(Opt)
self.joint_dist_lower_lim = self.joint_limits_lower - self.joint_values
self.joint_dist_upper_lim = self.joint_limits_upper - self.joint_values
self.lbA = np.hstack((error_posit, error_orient,
self.joint_dist_lower_lim, ac_lim_lower))
self.ubA = np.hstack((error_posit, error_orient,
self.joint_dist_upper_lim, ac_lim_upper))
# Recalculate H matrix
self.calculate_weigths(error_posit)
vel_calculation = SQProblem(self.problem_size[1],
self.problem_size[0])
vel_calculation.setOptions(options)
return_value = vel_calculation.init(self.H, self.g, self.A,
self.lb, self.ub, self.lbA,
self.ubA, nWSR)
vel_calculation.getPrimalSolution(Opt)
if return_value != returnValue.SUCCESSFUL_RETURN:
rospy.logerr("QP-Problem returned without success! ")
print 'joint limits: ', self.joint_limits_lower
print 'joint values: ', self.joint_values
print 'position error:', error_posit / self.prop_gain
print 'eef ori: [%.3f %.3f %.3f] ' % (
self.eef_pose.p[0], self.eef_pose.p[1], self.eef_pose.p[2])
print 'goal ori : [%.3f %.3f %.3f] ' % (
self.goal_orient_euler[0],
self.goal_orient_euler[0],
self.goal_orient_euler[0],
)
self.abort_goal('infeasible')
break
for x, vel in enumerate(Opt[4:self.nJoints]):
velocity_msg.velocity[self.start_arm[x]] = vel
velocity_msg.velocity[self.triang_list_pos] = Opt[3]
velocity_msg.velocity[0] = Opt[0]
velocity_msg.velocity[1] = Opt[1]
# Recalculate Error in EEF position
error_posit, limit_p = self.calc_posit_error(error_posit)
# Recalculate Error in EEF orientation
eef_orient = qo.rotation_to_quaternion(self.eef_pose.M)
error_orient, reached_orientation = self.calc_orient_error(
eef_orient, self.goal_quat, 0.35, limit_p)
# Get trajectory length
self.trajectory_length = self.get_trajectory_length(
self.eef_pose.p, self.trajectory_length)
# Print velocity for iai_naive_kinematics_sim
velocity_msg.header.stamp = rospy.get_rostime()
self.pub_velocity.publish(velocity_msg)
clock.now = rospy.get_rostime()
self.pub_clock.publish(clock)
# Action feedback
self.action_status.status = 1
self._feedback.sim_status = self.action_status.text = 'Calculating trajectory'
self._feedback.sim_trajectory.append(self.current_state)
self._feedback.trajectory_length = self.trajectory_length
self.action_server.publish_feedback(self.action_status,
self._feedback)
self.action_server.publish_status()
self.success = True
# Store EEF pose for plotting
eef_pose_msg.position.x = self.eef_pose.p[0]
eef_pose_msg.position.y = self.eef_pose.p[1]
eef_pose_msg.position.z = self.eef_pose.p[2]
eef_pose_msg.orientation.x = eef_orient[0]
eef_pose_msg.orientation.y = eef_orient[1]
eef_pose_msg.orientation.z = eef_orient[2]
eef_pose_msg.orientation.w = eef_orient[3]
eef_pose_array.poses.append(eef_pose_msg)
# Plot for debuging
'''for x in range(8+3):
low[x] = np.hstack((low[x], self.joint_limits_lower[x]))
pos[x] = np.hstack((pos[x], self.joint_values[x]))
vel_p[x] = np.hstack((vel_p[x], Opt[x]))
high[x] = np.hstack((high[x], self.joint_limits_upper[x]))
e = error_posit / self.prop_gain
e_p = sqrt(pow(e[0], 2) + pow(e[1], 2) + pow(e[2], 2))
error = np.hstack((error, e_p))
base_weight = np.hstack((base_weight, self.jweights[0, 0]))
arm_weight = np.hstack((arm_weight, self.jweights[4, 4]))
triang_weight = np.hstack((triang_weight, self.jweights[3, 3]))
t = np.hstack((t, i))#'''
# print '\n iter: ', i
# goal = euler_from_quaternion(self.goal_quat)
# print 'joint_vel: ', Opt[:-6]
# print 'error pos: ', error_posit / self.prop_gain
# print 'eef ori: [%.3f %.3f %.3f] '%(self.eef_pose.p[0],self.eef_pose.p[1], self.eef_pose.p[2])
# print 'ori error: [%.3f %.3f %.3f] '%(error_orient[0], error_orient[1], error_orient[2])
# print 'goal ori : [%.3f %.3f %.3f] '%(goal[0], goal[1], goal[2])
# print 'slack : ', Opt[-6:]
self.final_error = sqrt(
pow(error_posit[0], 2) + pow(error_posit[1], 2) +
pow(error_posit[2], 2))
eef_pose_array.header.stamp = rospy.get_rostime()
eef_pose_array.header.frame_id = self.gripper
self.pub_plot.publish(eef_pose_array)
# Plot
'''plt.style.use('ggplot')
plt.plot(t, arm_weight, 'g-.', lw=3, label='arm')
plt.plot(t, base_weight, 'k--', lw=3, label='base')
plt.plot(t, triang_weight, 'm', lw=2, label='torso')
plt.xlabel('Iterations')
plt.ylabel('Weights')
plt.title('Arm, base and torso weights')
plt.grid(True)
plt.legend()
# plt.show()
tikz_save('weights.tex', figureheight='5cm', figurewidth='16cm')
plt.cla()
plt.plot(t, error, 'k', lw=2)
plt.xlabel('Iterations')
plt.ylabel('Position Error [m]')
plt.title('Position Error')
plt.grid(True)
plt.legend()
# plt.show()
tikz_save('error.tex', figureheight='4cm', figurewidth='16cm')
plt.cla()#'''
'''for x in range(8+3):
plt.plot(t, low[x], lw=1)
plt.plot(t, pos[x], lw=3)
plt.plot(t, vel_p[x], lw=3)
plt.plot(t, high[x], lw=1)
plt.xlabel('Iterations')
plt.ylabel('Position / Velocity')
plt.grid(True)
# plt.show()
tikz_save('joint'+str(x)+'.tex', figureheight='5cm', figurewidth='4.5cm')
plt.cla()'''
'''plt.plot(t, pos[0], 'r--', lw=3, label='x')
plt.plot(t, pos[1], 'b', lw=2, label='y')
plt.xlabel('Iterations')
plt.ylabel('Base Position [m]')
plt.title('Trajectory of the base [m]')
plt.grid(True)
plt.legend()
# plt.show()
tikz_save('base_pos.tex', figureheight='5cm', figurewidth='16cm')
plt.cla()
step = 2
plt.plot(t[0:-1:step], pos[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0')
plt.plot(t[0:-1:step], pos[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1')
plt.plot(t[0:-1:step], pos[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2')
plt.plot(t[0:-1:step], pos[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3')
plt.plot(t, pos[7], 'k-.', lw=3, label='j4')
plt.plot(t, pos[8], '--', lw=3, label='j5')
plt.plot(t, pos[9], lw=3, label='j6')
plt.xlabel('Iterations')
plt.ylabel('Position [rad]')
plt.title('Trajectory of the joints')
plt.grid(True)
plt.legend(loc='upper left')
tikz_save('joint_pos.tex', figureheight='8cm', figurewidth='16cm')
plt.cla()
plt.plot(t, vel_p[0], 'r--', lw=3, label='x')
plt.plot(t, vel_p[1], 'b',lw=2, label='y')
plt.xlabel('Iterations')
plt.ylabel('Base Velocity [m/sec]')
plt.title('Velocity of the base')
plt.grid(True)
plt.legend()
tikz_save('base_vel.tex', figureheight='5cm', figurewidth='16cm')
plt.cla()
plt.plot(t[0:-1:step], vel_p[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0')
plt.plot(t[0:-1:step], vel_p[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1')
plt.plot(t[0:-1:step], vel_p[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2')
plt.plot(t[0:-1:step], vel_p[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3')
plt.plot(t, vel_p[7], 'k-.', lw=3, label='j4')
plt.plot(t, vel_p[8], '--', lw=3, label='j5')
plt.plot(t, vel_p[9], lw=3, label='j6')
plt.xlabel('Iterations')
plt.ylabel('Arms Joints Velocity [rad/sec]')
plt.title("Velocity of the arm's joints")
plt.legend(loc='upper left')
plt.grid(True)
tikz_save('joint_vel.tex', figureheight='8cm', figurewidth='16cm')
plt.cla()#'''
toc = rospy.get_rostime()
print(toc.secs - tic.secs), 'sec Elapsed'
return 0
def main():
rospy.init_node('controller_2dof', anonymous=True)
rospy.Rate(10)
rospy.sleep(0.2)
# Setup data of QP.
# Weights.
w1 = 1e-3
w2 = 1e-5
w3 = 1 # slack (attractor)
# Joint limits.
q0_max = 0.05
q1_max = 0.15
# Links
l1 = 0.1
l2 = 0.2
l3 = 0.3
# Initial joint values.
q0_init = q0 = -0.04
q1_init = q1 = -0.08
# Joint target.
q_des = 0.65
# Slack limits.
e_max = 1000
# Velocity limits.(+-)
v0_max = 0.05
v1_max = 0.1
# Acceleration limits.(+-)
a0_max = 0.05 * 0.5
a1_max = 0.1 * 0.5
# Others
precision = 1e-3
p = 10
q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1
error = p * (q_des - q_eef)
vel_init = 0
nWSR = np.array([100])
# Acceleration
a0_const = (v0_max - a0_max) / v0_max
a1_const = (v1_max - a1_max) / v1_max
example = SQProblem(3, 5)
H = np.array([w1, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, w3]).reshape((3, 3))
A = np.array([
1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0,
0.0
]).reshape((5, 3))
g = np.array([0.0, 0.0, 0.0, 0.0])
lb = np.array([-v0_max, -v1_max, -e_max])
ub = np.array([v0_max, v1_max, e_max])
lbA = np.array([error, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max])
ubA = np.array([error, (q0_max - q0), (q1_max - q0), a0_max, a1_max])
# Setting up QProblem object
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.LOW
example.setOptions(options)
print("Init pos = %g, goal = %g, error = %g, q0 =%g, q1 = %g\n" %
(q_eef, q_des, error, q0, q1))
print A
i = 0
n = 0
limit = abs(error)
ok = False
Opt = np.zeros(3)
# Plotting
t = np.array(i)
pos_eef = np.array(q_eef)
pos_0 = np.array(q0)
pos_1 = np.array(q1)
vel_0 = np.array(vel_init)
vel_1 = np.array(vel_init)
vel_eef = np.array(vel_init)
p_error = np.array(error)
goal = np.array(q_des)
lim1 = np.array(q0_max)
lim2 = np.array(q1_max)
return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Init of QP-Problem returned without success! ERROR MESSAGE: "
return -1
while not rospy.is_shutdown():
while limit > precision and i < 400:
# Solve QP.
i += 1
nWSR = np.array([100])
lbA[0] = error
lbA[1] = -q0_max - q0
lbA[2] = -q1_max - q1
lbA[3] = a0_const * Opt[0] - a0_max
lbA[4] = a1_const * Opt[1] - a1_max
ubA[0] = error
ubA[1] = q0_max - q0
ubA[2] = q1_max - q1
ubA[3] = a0_const * Opt[0] + a0_max
ubA[4] = a1_const * Opt[1] + a1_max
return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: "
return -1
# Get and print solution of QP.
example.getPrimalSolution(Opt)
q0 += Opt[0] / 100
q1 += Opt[1] / 100
q_eef = l1 + l2 + l3 + q0 + q1
error = p * (q_des - q_eef)
limit = abs(error)
# print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, error= %g, q0= %g q1= %g \n" % (
# Opt[0], Opt[1], Opt[2], Opt[3], q_eef, error, q0, q1)
# Plotting arrays
pos_eef = np.hstack((pos_eef, q_eef))
pos_0 = np.hstack((pos_0, q0))
pos_1 = np.hstack((pos_1, q1))
vel_0 = np.hstack((vel_0, Opt[0]))
vel_1 = np.hstack((vel_1, Opt[1]))
vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1]))
goal = np.hstack((goal, q_des))
p_error = np.hstack((p_error, error))
t = np.hstack((t, i))
lim1 = np.hstack((lim1, q0_max))
lim2 = np.hstack((lim2, q1_max))
# Plot
t_eef = go.Scatter(y=pos_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_eef',
line=dict(dash='dash'))
t_p0 = go.Scatter(y=pos_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_q0',
line=dict(dash='dash'))
t_p1 = go.Scatter(y=pos_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_q1',
line=dict(dash='dash'))
t_v0 = go.Scatter(y=vel_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_q0')
t_v1 = go.Scatter(y=vel_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_q1')
t_q0 = go.Scatter(y=lim1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='limit_0')
t_q1 = go.Scatter(y=lim2,
x=t,
marker=dict(size=4, ),
mode='lines',
name='limit_1')
t_veef = go.Scatter(y=vel_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_eef')
t_er = go.Scatter(y=p_error,
x=t,
marker=dict(size=4, ),
mode='lines+markers',
name='error')
t_goal = go.Scatter(y=goal,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal',
line=dict(dash='dot'))
print "\n i = %g \n" % (i)
data_eef = [t_eef, t_veef, t_goal]
layout_eef = dict(title="Initial position EEF = %g. Goal = %g, \n" %
(q_eef_init, q_des),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=2,
),
yaxis=dict(title='Position / Velocity'),
font=dict(size=16))
fig = dict(data=data_eef, layout=layout_eef)
plotly.offline.plot(fig,
filename='html/basic_example_eef.html',
image='png',
image_filename='basic_eef')
data = [t_p0, t_v0]
layout = dict(title="Initial position q0 =%g, q1 = %g.\n" %
(q0_init, q1_init),
xaxis=dict(title='Iterations',
autotick=False,
dtick=25,
gridwidth=2),
yaxis=dict(
title='Position / Velocity',
range=[-0.08, .12],
),
font=dict(size=18))
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/basic_example_q0.html',
image='png',
image_filename='basic_0')
data = [t_p1, t_v1]
layout = dict(title="Initial position q0 =%g, q1 = %g.\n" %
(q0_init, q1_init),
xaxis=dict(title='Iterations',
autotick=False,
dtick=25,
gridwidth=2),
yaxis=dict(
title='Position / Velocity',
range=[-0.08, .12],
),
font=dict(size=18))
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/basic_example_q1.html',
image='png',
image_filename='basic_1')
return 0
def test_example2(self):
# Example for qpOASES main function using the SQProblem 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.
H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
A_new = np.array([ 1.0, 5.0 ]).reshape((2,1))
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 SQProblem object and solution analyser.
qp = SQProblem(2, 1)
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
analyser = SolutionAnalysis()
# get c++ solution from std
cmd = os.path.join(bin_path, "example2")
p = Popen(cmd, shell=True, stdout=PIPE)
stdout, stderr = p.communicate()
stdout = str(stdout).replace('\\n', '\n')
stdout = stdout.replace("'", '')
print(stdout)
# Solve first QP ...
nWSR = 10
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
# ... and analyse it.
maxKKTviolation = np.zeros(1)
analyser.getMaxKKTviolation(qp, maxKKTviolation)
print("maxKKTviolation: %e\n"%maxKKTviolation)
actual = np.asarray(maxKKTviolation)
pattern = re.compile(r'maxKKTviolation: (?P<maxKKTviolation>[0-9+-e.]*)')
match = pattern.findall(stdout)
expected = np.asarray(match[0], dtype=float)
assert_almost_equal(actual, expected, decimal=7)
# Solve second QP ...
nWSR = 10
qp.hotstart(H_new, g_new, A_new,
lb_new, ub_new,
lbA_new, ubA_new, nWSR)
# ... and analyse it.
analyser.getMaxKKTviolation(qp, maxKKTviolation)
print("maxKKTviolation: %e\n"%maxKKTviolation)
actual = np.asarray(maxKKTviolation)
expected = np.asarray(match[1], dtype=float)
assert_almost_equal(actual, expected, decimal=7)
# ------------ VARIANCE-COVARIANCE EVALUATION --------------------
Var = np.zeros(5*5)
Primal_Dual_Var = np.zeros(5*5)
Var.reshape((5,5))[0,0] = 1.
Var.reshape((5,5))[1,1] = 1.
# ( 1 0 0 0 0 )
# ( 0 1 0 0 0 )
# Var = ( 0 0 0 0 0 )
# ( 0 0 0 0 0 )
# ( 0 0 0 0 0 )
analyser.getVarianceCovariance(qp, Var, Primal_Dual_Var)
print('Primal_Dual_Var=\n', Primal_Dual_Var.reshape((5,5)))
actual = Primal_Dual_Var.reshape((5,5))
pattern = re.compile(r'Primal_Dual_VAR = (?P<VAR>.*)',
re.DOTALL)
print(stdout)
match = pattern.search(stdout)
expected = match.group('VAR').strip().split("\n")
expected = [x.strip().split() for x in expected]
print(expected)
expected = np.asarray(expected, dtype=float)
assert_almost_equal(actual, expected, decimal=7)
def test_example1b(self):
"""Example for qpOASES main function using the QProblemB class."""
# Setup data of first QP.
H = np.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2))
g = np.array([1.5, 1.0])
lb = np.array([0.5, -2.0])
ub = np.array([5.0, 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])
# Setting up QProblemB object.
qp = QProblemB(2)
options = Options()
# options.enableFlippingBounds = BooleanType.FALSE
options.initialStatusBounds = SubjectToStatus.INACTIVE
options.numRefinementSteps = 1
options.enableCholeskyRefactorisation = 1
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 10
qp.init(H, g, lb, ub, nWSR)
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)
print 'xOpt_actual:', xOpt_actual
print 'objVal_actual:', objVal_actual
# Solve second QP.
nWSR = 10
qp.hotstart(g_new, lb_new, ub_new, nWSR)
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)
print 'xOpt_actual:', xOpt_actual
print 'objVal_actual:', objVal_actual
# 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, "example1b")
p = Popen(cmd, shell=True, stdout=PIPE)
stdout, stderr = p.communicate()
stdout = str(stdout).replace('\\n', '\n')
stdout = stdout.replace("'", '')
# get c++ solution from std
pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
match = pattern.findall(stdout)
xOpt_expected = match[-1][0]
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.findall(stdout)
print match
objVal_expected = match[-1]
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)
def test_qp_setup_with_toy_example_hack_from_qpoases_manual(self):
gen = ClassicGenerator()
options = Options()
options.printLevel = PrintLevel.LOW
# define matrices from qpoases manual
H = numpy.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2))
A = numpy.array([1.0, 1.0]).reshape((1, 2))
g = numpy.array([1.5, 1.0])
lb = numpy.array([0.5, -2.0])
ub = numpy.array([5.0, 2.0])
lbA = numpy.array([-1.0])
ubA = numpy.array([2.0])
x = numpy.array([0.5, -1.5])
f = -6.25e-02
# hack pattern generator
gen.ori_nv = 2
gen.ori_nc = 1
gen.ori_dofs = numpy.zeros((2, ))
gen.ori_qp = SQProblem(gen.ori_nv, gen.ori_nc)
gen.ori_qp.setOptions(options)
gen.ori_H = H
gen.ori_A = A
gen.ori_g = g
gen.ori_lb = lb
gen.ori_ub = ub
gen.ori_lbA = lbA
gen.ori_ubA = ubA
gen.pos_nv = 2
gen.pos_nc = 1
gen.pos_dofs = numpy.zeros((2, ))
gen.pos_qp = SQProblem(gen.pos_nv, gen.pos_nc)
gen.pos_qp.setOptions(options)
gen.pos_H = H
gen.pos_A = A
gen.pos_g = g
gen.pos_lb = lb
gen.pos_ub = ub
gen.pos_lbA = lbA
gen.pos_ubA = ubA
# test first qp solution
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
# define matrices for warmstart
H_new = numpy.array([1.0, 0.5, 0.5, 0.5]).reshape((2, 2))
A_new = numpy.array([1.0, 5.0]).reshape((1, 2))
g_new = numpy.array([1.0, 1.5])
lb_new = numpy.array([0.0, -1.0])
ub_new = numpy.array([5.0, -0.5])
lbA_new = numpy.array([-2.0])
ubA_new = numpy.array([1.0])
x = numpy.array([0.5, -0.5])
f = -1.875e-01
# hack pattern generator
gen.ori_H = H_new
gen.ori_A = A_new
gen.ori_g = g_new
gen.ori_lb = lb_new
gen.ori_ub = ub_new
gen.ori_lbA = lbA_new
gen.pos_H = H_new
gen.pos_A = A_new
gen.pos_g = g_new
gen.pos_lb = lb_new
gen.pos_ub = ub_new
gen.pos_lbA = lbA_new
# test qp warmstart
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
def solveLeastSquare(A,
b,
lb=None,
ub=None,
A_in=None,
lb_in=None,
ub_in=None,
maxIterations=None,
maxComputationTime=60.0,
regularization=1e-8):
n = A.shape[1]
m_in = 0
A = np.asarray(A)
b = np.asarray(b).squeeze()
if (A_in is not None):
m_in = A_in.shape[0]
if (lb_in is None):
lb_in = np.array(m_in * [-1e99])
else:
lb_in = np.asarray(lb_in).squeeze()
if (ub_in is None):
ub_in = np.array(m_in * [1e99])
else:
ub_in = np.asarray(ub_in).squeeze()
if (lb is None):
lb = np.array(n * [-1e99])
else:
lb = np.asarray(lb).squeeze()
if (ub is None):
ub = np.array(n * [1e99])
else:
ub = np.asarray(ub).squeeze()
# 0.5||Ax-b||^2 = 0.5(x'A'Ax - 2b'Ax + b'b) = 0.5x'A'Ax - b'Ax +0.5b'b
Hess = np.dot(A.T, A) + regularization * np.identity(n)
grad = -np.dot(A.T, b)
if (maxIterations is None):
maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n])
else:
maxActiveSetIter = np.array([maxIterations])
maxComputationTime = np.array([maxComputationTime])
options = Options()
options.printLevel = PrintLevel.NONE
#NONE, LOW, MEDIUM
options.enableRegularisation = False
if (m_in == 0):
qpOasesSolver = QProblemB(n)
#, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
# beware that the Hessian matrix may be modified by this function
imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter,
maxComputationTime)
else:
qpOasesSolver = SQProblem(n, m_in)
#, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in,
maxActiveSetIter, maxComputationTime)
x = np.empty(n)
qpOasesSolver.getPrimalSolution(x)
#print "QP cost:", 0.5*(np.linalg.norm(np.dot(A, x)-b)**2);
return (imode, np.asmatrix(x).T)
def test_example1b(self):
"""Example for qpOASES main function using the QProblemB class."""
# Setup data of first QP.
H = np.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2))
g = np.array([1.5, 1.0])
lb = np.array([0.5, -2.0])
ub = np.array([5.0, 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])
# Setting up QProblemB object.
qp = QProblemB(2)
options = Options()
# options.enableFlippingBounds = BooleanType.FALSE
options.initialStatusBounds = SubjectToStatus.INACTIVE
options.numRefinementSteps = 1
options.enableCholeskyRefactorisation = 1
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 10
qp.init(H, g, lb, ub, nWSR)
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)
print 'xOpt_actual:', xOpt_actual
print 'objVal_actual:', objVal_actual
# Solve second QP.
nWSR = 10
qp.hotstart(g_new, lb_new, ub_new, nWSR)
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)
print 'xOpt_actual:', xOpt_actual
print 'objVal_actual:', objVal_actual
# 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, "example1b")
p = Popen(cmd, shell=True, stdout=PIPE)
stdout, stderr = p.communicate()
stdout = str(stdout).replace('\\n', '\n')
stdout = stdout.replace("'", '')
# get c++ solution from std
pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
match = pattern.findall(stdout)
xOpt_expected = match[-1][0]
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.findall(stdout)
print match
objVal_expected = match[-1]
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)
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
def __init__(self, N=16, T=0.1, T_step=0.8, fsm_state='D', fsm_sl=1):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(ClassicGenerator, self).__init__(N, T, T_step, fsm_state, fsm_sl)
# TODO for speed up one can define members of BaseGenerator as
# direct views of QP data structures according to walking report
# Maybe do that later!
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x^T * H(w0) * x + x^T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # number of working set recalculations
self.options = Options()
self.options.setToMPC()
self.options.printLevel = PrintLevel.LOW
# FOR ORIENTATIONS
# define dimensions
self.ori_nv = 2 * self.N
self.ori_nc = (self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq)
# setup problem
self.ori_dofs = numpy.zeros(self.ori_nv)
self.ori_qp = SQProblem(self.ori_nv, self.ori_nc)
self.ori_qp.setOptions(self.options) # load NMPC options
self.ori_H = numpy.zeros((self.ori_nv, self.ori_nv))
self.ori_A = numpy.zeros((self.ori_nc, self.ori_nv))
self.ori_g = numpy.zeros((self.ori_nv, ))
self.ori_lb = -numpy.ones((self.ori_nv, )) * 1e+08
self.ori_ub = numpy.ones((self.ori_nv, )) * 1e+08
self.ori_lbA = -numpy.ones((self.ori_nc, )) * 1e+08
self.ori_ubA = numpy.ones((self.ori_nc, )) * 1e+08
self._ori_qp_is_initialized = False
# save computation time and working set recalculations
self.ori_qp_nwsr = 0.0
self.ori_qp_cputime = 0.0
# FOR POSITIONS
# define dimensions
self.pos_nv = 2 * (self.N + self.nf)
self.pos_nc = (self.nc_cop + self.nc_foot_position +
self.nc_fchange_eq)
# setup problem
self.pos_dofs = numpy.zeros(self.pos_nv)
self.pos_qp = SQProblem(self.pos_nv, self.pos_nc)
self.pos_qp.setOptions(self.options)
self.pos_H = numpy.zeros((self.pos_nv, self.pos_nv))
self.pos_A = numpy.zeros((self.pos_nc, self.pos_nv))
self.pos_g = numpy.zeros((self.pos_nv, ))
self.pos_lb = -numpy.ones((self.pos_nv, )) * 1e+08
self.pos_ub = numpy.ones((self.pos_nv, )) * 1e+08
self.pos_lbA = -numpy.ones((self.pos_nc, )) * 1e+08
self.pos_ubA = numpy.ones((self.pos_nc, )) * 1e+08
self._pos_qp_is_initialized = False
# save computation time and working set recalculations
self.pos_qp_nwsr = 0.0
self.pos_qp_cputime = 0.0
# dummy matrices
self._ori_Q = numpy.zeros((2 * self.N, 2 * self.N))
self._ori_p = numpy.zeros((2 * self.N, ))
self._pos_Q = numpy.zeros((self.N + self.nf, self.N + self.nf))
self._pos_p = numpy.zeros((self.N + self.nf, ))
# add additional keys that should be saved
self._data_keys.append('ori_qp_nwsr')
self._data_keys.append('ori_qp_cputime')
self._data_keys.append('pos_qp_nwsr')
self._data_keys.append('pos_qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
class ClassicGenerator(BaseGenerator):
"""
Reimplementation of current state-of-the-art pattern
generator for HRP-2 of CNRS-LAAS, Toulouse.
Solve QPs for position and orientation of CoM and feet
independently of each other in each timestep.
First solve for orientations, then solve for the postions.
"""
def __init__(
self, N=16, T=0.1, T_step=0.8,
fsm_state='D', fsm_sl=1
):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(ClassicGenerator, self).__init__(
N, T, T_step, fsm_state, fsm_sl
)
# TODO for speed up one can define members of BaseGenerator as
# direct views of QP data structures according to walking report
# Maybe do that later!
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x^T * H(w0) * x + x^T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # number of working set recalculations
self.options = Options()
self.options.setToMPC()
self.options.printLevel = PrintLevel.LOW
# FOR ORIENTATIONS
# define dimensions
self.ori_nv = 2*self.N
self.ori_nc = (
self.nc_fvel_eq
+ self.nc_fpos_ineq
+ self.nc_fvel_ineq
)
# setup problem
self.ori_dofs = numpy.zeros(self.ori_nv)
self.ori_qp = SQProblem(self.ori_nv, self.ori_nc)
self.ori_qp.setOptions(self.options) # load NMPC options
self.ori_H = numpy.zeros((self.ori_nv,self.ori_nv))
self.ori_A = numpy.zeros((self.ori_nc,self.ori_nv))
self.ori_g = numpy.zeros((self.ori_nv,))
self.ori_lb = -numpy.ones((self.ori_nv,))*1e+08
self.ori_ub = numpy.ones((self.ori_nv,))*1e+08
self.ori_lbA = -numpy.ones((self.ori_nc,))*1e+08
self.ori_ubA = numpy.ones((self.ori_nc,))*1e+08
self._ori_qp_is_initialized = False
# save computation time and working set recalculations
self.ori_qp_nwsr = 0.0
self.ori_qp_cputime = 0.0
# FOR POSITIONS
# define dimensions
self.pos_nv = 2*(self.N + self.nf)
self.pos_nc = (
self.nc_cop
+ self.nc_foot_position
+ self.nc_fchange_eq
)
# setup problem
self.pos_dofs = numpy.zeros(self.pos_nv)
self.pos_qp = SQProblem(self.pos_nv, self.pos_nc)
self.pos_qp.setOptions(self.options)
self.pos_H = numpy.zeros((self.pos_nv,self.pos_nv))
self.pos_A = numpy.zeros((self.pos_nc,self.pos_nv))
self.pos_g = numpy.zeros((self.pos_nv,))
self.pos_lb = -numpy.ones((self.pos_nv,))*1e+08
self.pos_ub = numpy.ones((self.pos_nv,))*1e+08
self.pos_lbA = -numpy.ones((self.pos_nc,))*1e+08
self.pos_ubA = numpy.ones((self.pos_nc,))*1e+08
self._pos_qp_is_initialized = False
# save computation time and working set recalculations
self.pos_qp_nwsr = 0.0
self.pos_qp_cputime = 0.0
# dummy matrices
self._ori_Q = numpy.zeros((2*self.N, 2*self.N))
self._ori_p = numpy.zeros((2*self.N,))
self._pos_Q = numpy.zeros((self.N + self.nf, self.N + self.nf))
self._pos_p = numpy.zeros((self.N + self.nf,))
# add additional keys that should be saved
self._data_keys.append('ori_qp_nwsr')
self._data_keys.append('ori_qp_cputime')
self._data_keys.append('pos_qp_nwsr')
self._data_keys.append('pos_qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
def solve(self):
""" Process and solve problem, s.t. pattern generator data is consistent """
self._preprocess_solution()
self._solve_qp()
self._postprocess_solution()
def _preprocess_solution(self):
""" Update matrices and get them into the QP data structures """
# rename for convenience
N = self.N
nf = self.nf
# ORIENTATIONS
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._ori_qp_is_initialized:
# ori_dofs = ( dddF_k_qR )
# ( dddF_k_qL )
self.ori_dofs[:N] = self.dddF_k_qR
self.ori_dofs[N:] = self.dddF_k_qL
# TODO guess initial active set
# this requires changes to the python interface
# define QP matrices
# H = ( Q_k_q )
self._update_ori_Q() # updates values in _Q
self.ori_H [:,:] = self._ori_Q
# g = ( p_k_q )
self._update_ori_p() # updates values in _p
self.ori_g [:] = self._ori_p
# ORIENTATION LINEAR CONSTRAINTS
# velocity constraints on support foot to freeze movement
a = 0
b = self.nc_fvel_eq
self.ori_A [a:b] = self.A_fvel_eq
self.ori_lbA[a:b] = self.B_fvel_eq
self.ori_ubA[a:b] = self.B_fvel_eq
# box constraints for maximum orientation change
a = self.nc_fvel_eq
b = self.nc_fvel_eq + self.nc_fpos_ineq
self.ori_A [a:b] = self.A_fpos_ineq
self.ori_lbA[a:b] = self.lbB_fpos_ineq
self.ori_ubA[a:b] = self.ubB_fpos_ineq
# box constraints for maximum angular velocity
a = self.nc_fvel_eq + self.nc_fpos_ineq
b = self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq
self.ori_A [a:b] = self.A_fvel_ineq
self.ori_lbA[a:b] = self.lbB_fvel_ineq
self.ori_ubA[a:b] = self.ubB_fvel_ineq
# ORIENTATION BOX CONSTRAINTS
#self.ori_lb [...] = 0.0
#self.ori_ub [...] = 0.0
# POSITIONS
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._pos_qp_is_initialized:
# pos_dofs = ( dddC_kp1_x )
# ( F_k_x )
# ( dddC_kp1_y )
# ( F_k_y )
self.pos_dofs[0 :0+N ] = self.dddC_k_x
self.pos_dofs[0+N:0+N+nf] = self.F_k_x
a = N+nf
self.pos_dofs[a :a+N ] = self.dddC_k_y
self.pos_dofs[a+N:a+N+nf] = self.F_k_y
# TODO guess initial active set
# this requires changes to the python interface
# define QP matrices
# H = ( Q_k 0 )
# ( 0 Q_k )
self._update_pos_Q() # updates values in _Q
self.pos_H [ :N+nf, :N+nf] = self._pos_Q
self.pos_H [-N-nf:, -N-nf:] = self._pos_Q
# g = ( p_k_x )
# ( p_k_y )
self._update_pos_p('x') # updates values in _p
self.pos_g [ :N+nf] = self._pos_p
self._update_pos_p('y') # updates values in _p
self.pos_g [-N-nf:] = self._pos_p
# lbA <= A x <= ubA
# ( ) <= ( Acop ) <= ( Bcop )
# (eqBfoot) <= ( eqAfoot ) <= ( eqBfoot )
# ( ) <= ( Afoot ) <= ( Bfoot )
# CoP constraints
a = 0
b = self.nc_cop
self.pos_A [a:b] = self.Acop
self.pos_lbA[a:b] = self.lbBcop
self.pos_ubA[a:b] = self.ubBcop
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.pos_A [a:b] = self.Afoot
self.pos_lbA[a:b] = self.lbBfoot
self.pos_ubA[a:b] = self.ubBfoot
#foot equality constraints
a = self.nc_cop + self.nc_foot_position
b = self.nc_cop + self.nc_foot_position + self.nc_fchange_eq
self.pos_A [a:b] = self.eqAfoot
self.pos_lbA[a:b] = self.eqBfoot
self.pos_ubA[a:b] = self.eqBfoot
# NOTE: they stay plus minus infinity, i.e. 1e+08
#self.pos_lb [...] = 0.0
#self.pos_ub [...] = 0.0
def _update_ori_Q(self):
'''
Update Hessian block Q according to walking report
min a/2 || dC_kp1_q_ref - dF_k_q ||_2^2
Q = ( QR 0 )
( 0 QL )
QR = ( a * Pvu^T * E_FR^T * E_FR * Pvu )
'''
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
Pvu = self.Pvu
# assemble Hessian matrix
# QR = ( a * Pvu^T * E_FR^T * E_FR * Pvu )
a = 0; b = N
c = 0; d = N
QR = self._ori_Q[a:b,c:d]
QR[...] = alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR).dot(Pvu)
# QL = ( a * Pvu^T * E_FL^T * E_FL * Pvu )
# Q = ( * , * )
# ( * ,[*])
a = N; b = 2*N
c = N; d = 2*N
QL = self._ori_Q[a:b,c:d]
QL[...] = alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL).dot(Pvu)
def _update_ori_p(self):
"""
Update pass gradient block p according to walking report
min a/2 || dC_kp1_q_ref - dF_k_q ||_2^2
p = ( pR )
( pL )
pR = ( a * Pvu^T * E_FR^T * (E_FR * Pvs * f_k_qR - dC_kp1_q_ref[N/2] )
"""
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
f_k_qR = self.f_k_qR
f_k_qL = self.f_k_qL
Pvs = self.Pvs
Pvu = self.Pvu
dC_kp1_q_ref = self.dC_kp1_q_ref
# pR = ( a * Pvu^T * E_FL^T * (E_FL * Pvs * f_k_qL + dC_kp1_q_ref) )
# p = ([*]) =
# ( * )
a = 0; b = N
self._ori_p[a:b] = \
alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR.dot(Pvs).dot(f_k_qR) - dC_kp1_q_ref)
# p = ( * ) =
# ([*])
a = N; b = 2*N
self._ori_p[a:b] = \
alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL.dot(Pvs).dot(f_k_qL) - dC_kp1_q_ref)
def _update_pos_Q(self):
'''
Update Hessian block Q according to walking report
Q = ( a*Pvu*Pvu + b*Ppu*E*T*E*Ppu + c*Pzu*Pzu + d*I, -c*Pzu*V_kp1 )
( -c*Pzu*V_kp1, c*V_kp1*V_kp1 )
'''
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# cast matrices for convenience
Ppu = numpy.asmatrix(self.Ppu)
Pvu = numpy.asmatrix(self.Pvu)
Pzu = numpy.asmatrix(self.Pzu)
V_kp1 = numpy.asmatrix(self.V_kp1)
# Q = ([*], * ) = a*Pvu*Pvu + b*Ppu*E*E*Ppu + c*Pzu*Pzu + d*I
# ( * , * )
a = 0; b = N
c = 0; d = N
self._pos_Q[a:b,c:d] = alpha * Pvu.transpose() * Pvu \
+ gamma * Pzu.transpose() * Pzu \
+ delta * numpy.eye(N)
# Q = ( * ,[*])
# ( * , * )
a = 0; b = N
c = N; d = N+nf
self._pos_Q[a:b,c:d] = -gamma * Pzu.transpose() * V_kp1
# Q = ( * , * ) = ( * , [*] )^T
# ( [*], * ) ( * , * )
dummy = self._pos_Q[a:b,c:d]
a = N; b = N+nf
c = 0; d = N
self._pos_Q[a:b,c:d] = dummy.transpose()
# Q = ( * , * )
# ( * ,[*])
a = N; b = N+nf
c = N; d = N+nf
self._pos_Q[a:b,c:d] = gamma * V_kp1.transpose() * V_kp1
def _update_pos_p(self, case=None):
"""
Update pass gradient block p according to walking report
p = ( a*Pvu*(Pvs*ck - Refk+1) + b*Ppu*E*(E*Pps*cx - Refk+1) + c*Pzu*(Pzs*ck - vk+1*fk )
( -c*Vk+1*(Pzs*ck - vk+1*fk )
"""
if case == 'x':
f_k = self.f_k_x
c_k = utility.cast_array_as_matrix(self.c_k_x)
dC_kp1_ref = utility.cast_array_as_matrix(self.dC_kp1_x_ref)
elif case == 'y':
f_k = self.f_k_y
c_k = utility.cast_array_as_matrix(self.c_k_y)
dC_kp1_ref = utility.cast_array_as_matrix(self.dC_kp1_y_ref)
else:
err_str = 'Please use either case "x" or "y" for this routine'
raise AttributeError(err_str)
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
# matrices
v_kp1 = utility.cast_array_as_matrix(self.v_kp1)
Pvs = numpy.asmatrix(self.Pvs)
Pvu = numpy.asmatrix(self.Pvu)
Pps = numpy.asmatrix(self.Pps)
Ppu = numpy.asmatrix(self.Ppu)
Pzs = numpy.asmatrix(self.Pzs)
Pzu = numpy.asmatrix(self.Pzu)
V_kp1 = numpy.asmatrix(self.V_kp1)
# p = ([*]) =
# ( * )
a = 0; b = N
self._pos_p[a:b] = (
alpha * Pvu.transpose() *(Pvs*c_k - dC_kp1_ref)
+ gamma * Pzu.transpose() *(Pzs*c_k - v_kp1*f_k)
#+ b*Ppu.transpose() * E.transpose() * E * Ppu \
).ravel()
# p = ( * ) =
# ([*])
a = N; b = N+nf
self._pos_p[a:b] = (
-gamma * V_kp1.transpose() * (Pzs*c_k - v_kp1*f_k)
).ravel()
def _solve_qp(self):
"""
Solve QP first run with init functionality and other runs with warmstart
"""
#sys.stdout.write('Solve for orientations:\n')
if not self._ori_qp_is_initialized:
ret, nwsr, cputime = self.ori_qp.init(
self.ori_H, self.ori_g, self.ori_A,
self.ori_lb, self.ori_ub,
self.ori_lbA, self.ori_ubA,
self.nwsr, self.cpu_time
)
self._ori_qp_is_initialized = True
else:
ret, nwsr, cputime = self.ori_qp.hotstart(
self.ori_H, self.ori_g, self.ori_A,
self.ori_lb, self.ori_ub,
self.ori_lbA, self.ori_ubA,
self.nwsr, self.cpu_time
)
# orientation primal solution
self.ori_qp.getPrimalSolution(self.ori_dofs)
# save qp solver data
self.ori_qp_nwsr = nwsr # working set recalculations
self.ori_qp_cputime = cputime*1000. # in milliseconds
#sys.stdout.write('Solve for positions:\n')
if not self._pos_qp_is_initialized:
ret, nwsr, cputime = self.pos_qp.init(
self.pos_H, self.pos_g, self.pos_A,
self.pos_lb, self.pos_ub,
self.pos_lbA, self.pos_ubA,
self.nwsr, self.cpu_time
)
self._pos_qp_is_initialized = True
else:
ret, nwsr, cputime = self.pos_qp.hotstart(
self.pos_H, self.pos_g, self.pos_A,
self.pos_lb, self.pos_ub,
self.pos_lbA, self.pos_ubA,
self.nwsr, self.cpu_time
)
# position primal solution
self.pos_qp.getPrimalSolution(self.pos_dofs)
# save qp solver data
self.pos_qp_nwsr = nwsr # working set recalculations
self.pos_qp_cputime = cputime*1000. # in milliseconds
def _postprocess_solution(self):
""" Get solution and put it back into generator data structures """
# rename for convenience
N = self.N
nf = self.nf
# extract dofs
# ori_dofs = ( dddF_k_qR )
# ( dddF_k_qL )
self.dddF_k_qR[:] = self.ori_dofs[:N]
self.dddF_k_qL[:] = self.ori_dofs[N:]
# extract dofs
# pos_dofs = ( dddC_kp1_x )
# ( F_k_x )
# ( dddC_kp1_y )
# ( F_k_y )
self.dddC_k_x[:] = self.pos_dofs[0 :0+N ]
self.F_k_x[:] = self.pos_dofs[0+N:0+N+nf]
a = N + nf
self.dddC_k_y[:] = self.pos_dofs[a :a+N ]
self.F_k_y[:] = self.pos_dofs[a+N:a+N+nf]
def test_qp_setup_with_toy_example_hack_from_qpoases_manual(self):
gen = ClassicGenerator()
options = Options()
options.printLevel = PrintLevel.LOW
# define matrices from qpoases manual
H = numpy.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
A = numpy.array([ 1.0, 1.0 ]).reshape((1,2))
g = numpy.array([ 1.5, 1.0 ])
lb = numpy.array([ 0.5, -2.0 ])
ub = numpy.array([ 5.0, 2.0 ])
lbA = numpy.array([ -1.0 ])
ubA = numpy.array([ 2.0 ])
x = numpy.array([0.5, -1.5])
f = -6.25e-02
# hack pattern generator
gen.ori_nv = 2
gen.ori_nc = 1
gen.ori_dofs = numpy.zeros((2,))
gen.ori_qp = SQProblem(gen.ori_nv, gen.ori_nc)
gen.ori_qp.setOptions(options)
gen.ori_H = H
gen.ori_A = A
gen.ori_g = g
gen.ori_lb = lb
gen.ori_ub = ub
gen.ori_lbA = lbA
gen.ori_ubA = ubA
gen.pos_nv = 2
gen.pos_nc = 1
gen.pos_dofs = numpy.zeros((2,))
gen.pos_qp = SQProblem(gen.pos_nv, gen.pos_nc)
gen.pos_qp.setOptions(options)
gen.pos_H = H
gen.pos_A = A
gen.pos_g = g
gen.pos_lb = lb
gen.pos_ub = ub
gen.pos_lbA = lbA
gen.pos_ubA = ubA
# test first qp solution
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
# define matrices for warmstart
H_new = numpy.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
A_new = numpy.array([ 1.0, 5.0 ]).reshape((1,2))
g_new = numpy.array([ 1.0, 1.5 ])
lb_new = numpy.array([ 0.0, -1.0 ])
ub_new = numpy.array([ 5.0, -0.5 ])
lbA_new = numpy.array([ -2.0 ])
ubA_new = numpy.array([ 1.0 ])
x = numpy.array([0.5, -0.5])
f = -1.875e-01
# hack pattern generator
gen.ori_H = H_new
gen.ori_A = A_new
gen.ori_g = g_new
gen.ori_lb = lb_new
gen.ori_ub = ub_new
gen.ori_lbA = lbA_new
gen.pos_H = H_new
gen.pos_A = A_new
gen.pos_g = g_new
gen.pos_lb = lb_new
gen.pos_ub = ub_new
gen.pos_lbA = lbA_new
# test qp warmstart
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
def solve(self, i, vref):
"""build the problem and solve it
"""
#we build the N-sized reference vectors
if (i + self.N < self.ntime + 1):
#take the values stored in the rows (i to N)
vref_pred = vref[i:i + self.N, :]
else:
# If the matrix doesn't have N rows ahead, then repeat the last row to fill the void
vref_pred = np.vstack(
(vref[i:self.ntime, :],
np.dot(np.ones((self.N - self.ntime + i, 1)),
np.expand_dims(vref[self.ntime - 1, :], axis=0))))
#get condensed matrices (constant from on step to another)
[Sx, Sy, Svx, Svy, St, Svt, Scx, Scy, Ux, Uu, D_diag] = self.condense()
#build matrix V
V, V_dash = self.get_walk_matrices(i)
##################stuff for the solver###############################
#build constraint vectors (changing from one step to another)
#footsteps and CoP upper bounds
#ubounds CoP x position
ubounds1 = np.vstack(([self.CoP_ubounds[0] for _ in range(self.N)]))
ubounds1 = ubounds1 - np.dot(np.dot(Scx, D_diag), np.dot(
Ux, self.x.T)) + np.dot(V, self.f_current[0][0])
#ubounds CoP y position
ubounds2 = np.vstack([self.CoP_ubounds[1] for _ in range(self.N)])
ubounds2 = ubounds2 - np.dot(np.dot(Scy, D_diag), np.dot(
Ux, self.x.T)) + np.dot(V, self.f_current[0][1])
#ubounds feet x position
ubounds3 = np.vstack((self.feet_ubounds[0], self.feet_ubounds[2]))
ubounds3 = ubounds3 + np.vstack(
(np.dot(self.current_rotation[np.newaxis, 0, :],
self.f_current[0, 0:2, np.newaxis]),
np.zeros((self.future_steps - 1, 1))))
#ubounds feet y position
ubounds4 = np.vstack((self.feet_ubounds[1], self.feet_ubounds[3]))
ubounds4 = ubounds4 + np.vstack(
(np.dot(self.current_rotation[np.newaxis, 1, :],
self.f_current[0, 0:2, np.newaxis]),
np.zeros((self.future_steps - 1, 1))))
#ubounds feet angles
ubounds5 = self.feet_angle_ubound * np.ones((self.future_steps, 1))
ubounds5 = ubounds5 + np.vstack(
(self.f_current[np.newaxis, np.newaxis, 0,
2], np.zeros((self.future_steps - 1, 1))))
ubA = np.vstack((ubounds1, ubounds2, ubounds3, ubounds4, ubounds5))
#footsteps and CoP lower bounds
#lbounds CoP x position
lbounds1 = np.vstack([self.CoP_lbounds[0] for _ in range(self.N)])
lbounds1 = lbounds1 - np.dot(np.dot(Scx, D_diag), np.dot(
Ux, self.x.T)) + np.dot(V, self.f_current[0][0])
#lbounds CoP y position
lbounds2 = np.vstack([self.CoP_lbounds[1] for _ in range(self.N)])
lbounds2 = lbounds2 - np.dot(np.dot(Scy, D_diag), np.dot(
Ux, self.x.T)) + np.dot(V, self.f_current[0][1])
#lbounds feet x position
lbounds3 = np.vstack((self.feet_lbounds[0], self.feet_lbounds[2]))
lbounds3 = lbounds3 + np.vstack(
(np.dot(self.current_rotation[np.newaxis, 0, :],
self.f_current[0, 0:2, np.newaxis]),
np.zeros((self.future_steps - 1, 1))))
#lbounds feet y position
lbounds4 = np.vstack((self.feet_lbounds[1], self.feet_lbounds[3]))
lbounds4 = lbounds4 + np.vstack(
(np.dot(self.current_rotation[np.newaxis, 1, :],
self.f_current[0, 0:2, np.newaxis]),
np.zeros((self.future_steps - 1, 1))))
#lbounds feet angles
lbounds5 = self.feet_angle_lbound * np.ones((self.future_steps, 1))
lbounds5 = lbounds5 + np.vstack(
(self.f_current[np.newaxis, np.newaxis, 0,
2], np.zeros((self.future_steps - 1, 1))))
lbA = np.vstack((lbounds1, lbounds2, lbounds3, lbounds4, lbounds5))
#big A matrix (constraints matrix)
A = np.zeros((self.n_constraints, self.n_dec))
#A elements for constraints for CoP position
A[0:self.N, :] = np.hstack(
(np.dot(np.dot(Scx, D_diag),
Uu), -V_dash, np.zeros((self.N, 2 * self.future_steps))))
A[self.N:2 * self.N, :] = np.hstack(
(np.dot(np.dot(Scy, D_diag),
Uu), np.zeros((self.N, self.future_steps)), -V_dash,
np.zeros((self.N, self.future_steps))))
A[2 * self.N, 3 * self.N] = self.current_rotation[0, 0]
A[2 * self.N, 3 * self.N + 2] = self.current_rotation[0, 1]
A[2 * self.N + 1, 3 * self.N] = -self.current_rotation[0, 0]
A[2 * self.N + 1, 3 * self.N + 1] = self.current_rotation[0, 0]
A[2 * self.N + 1, 3 * self.N + 2] = -self.current_rotation[0, 1]
A[2 * self.N + 1, 3 * self.N + 3] = self.current_rotation[0, 1]
A[2 * self.N + 2, 3 * self.N] = self.current_rotation[1, 0]
A[2 * self.N + 2, 3 * self.N + 2] = self.current_rotation[1, 1]
A[2 * self.N + 3, 3 * self.N] = -self.current_rotation[1, 0]
A[2 * self.N + 3, 3 * self.N + 1] = self.current_rotation[1, 0]
A[2 * self.N + 3, 3 * self.N + 2] = -self.current_rotation[1, 1]
A[2 * self.N + 3, 3 * self.N + 3] = self.current_rotation[1, 1]
#max angle between feet constraints
A[2 * self.N + 4, 3 * self.N + 4] = 1
A[2 * self.N + 5, 3 * self.N + 4] = -1
A[2 * self.N + 5, 3 * self.N + 5] = 1
#QP solver takes one dim arrays
lbA = lbA.reshape((lbA.shape[0], ))
ubA = ubA.reshape((ubA.shape[0], ))
#############################################################################HESSIAN AND GRADIENT####################
H = np.zeros((self.n_dec, self.n_dec))
H[0:3*self.N, 0:3*self.N] = self.alpha*np.ones((3*self.N, 3*self.N)) + self.beta*np.dot(np.dot(Svx, Uu).T, np.dot(Svx, Uu)) + self.beta*np.dot(np.dot(Svy, Uu).T, np.dot(Svy, Uu)) + \
+ self.beta*np.dot(np.dot(Svt, Uu).T, np.dot(Svt, Uu)) + self.gamma*np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, np.dot(np.dot(Scx, D_diag), Uu)) + \
+ self.gamma*np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, np.dot(np.dot(Scy, D_diag), Uu)) + self.gamma*np.dot(np.dot(St, Uu).T, np.dot(St, Uu))
H[0:3 * self.N, 3 * self.N:] = np.hstack(
(-self.gamma * np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, V_dash),
-self.gamma * np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, V_dash),
-self.gamma * np.dot(np.dot(St, Uu).T, V_dash)))
H[3 * self.N:, 0:3 * self.N] = np.vstack(
(-self.gamma * np.dot(V_dash.T, np.dot(np.dot(Scx, D_diag), Uu)),
-self.gamma * np.dot(V_dash.T, np.dot(np.dot(Scy, D_diag), Uu)),
-self.gamma * np.dot(V_dash.T, np.dot(St, Uu))))
H[3 * self.N:, 3 * self.N:] = self.gamma * block_diag(
np.dot(V_dash.T, V_dash), np.dot(V_dash.T, V_dash),
np.dot(V_dash.T, V_dash))
g = np.zeros((self.n_dec, 1))
g[0:3*self.N, :] = self.beta*np.dot(np.dot(Svx, Uu).T, np.dot(np.dot(Svx, Ux), self.x.T) - vref_pred[:, 0][np.newaxis].T) + self.beta*np.dot(np.dot(Svy, Uu).T, np.dot(np.dot(Svy, Ux), self.x.T) - vref_pred[:, 1][np.newaxis].T) + \
self.beta*np.dot(np.dot(Svt, Uu).T, np.dot(np.dot(Svt, Ux), self.x.T) - vref_pred[:, 2][np.newaxis].T) + self.gamma*np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, np.dot(np.dot(np.dot(Scx, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][0])) + \
+ self.gamma*np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, np.dot(np.dot(np.dot(Scy, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][1])) + self.gamma*np.dot(np.dot(St, Uu).T, np.dot(np.dot(St, Ux), self.x.T)) - \
- self.gamma*np.dot(np.dot(St, Uu).T, np.dot(V, self.f_current[0][2]))
g[3 * self.N:50, :] = -self.gamma * np.dot(
V_dash.T,
np.dot(np.dot(np.dot(Scx, D_diag), Ux), self.x.T) -
np.dot(V, self.f_current[0][0]))
g[50:52, :] = -self.gamma * np.dot(
V_dash.T,
np.dot(np.dot(np.dot(Scy, D_diag), Ux), self.x.T) -
np.dot(V, self.f_current[0][1]))
g[52:, :] = -self.gamma * np.dot(
V_dash.T,
np.dot(np.dot(St, Ux), self.x.T) - np.dot(V, self.f_current[0][2]))
g = g.reshape((g.shape[0], ))
###########################################################################################################################
#solver options - MPC
myOptions = Options()
myOptions.setToMPC()
self.QP.setOptions(myOptions)
#setting lb and ub to huge numbers - otherwise solver gives errors
lb = -1000000000. * np.ones((self.n_dec, ))
ub = 1000000000. * np.ones((self.n_dec, ))
#solve QP - QP.init()
self.QP.init(H, g, A, lb, ub, lbA, ubA, nWSR=np.array([1000000]))
#get the solution (getPrimalSolution())
x_opt = np.zeros(self.n_dec)
self.QP.getPrimalSolution(x_opt)
#update the state of the system, CoP and the footstep (take the first elements of the solution)
self.controls = np.array([[x_opt[0], x_opt[1], x_opt[2]]])
self.x = (np.dot(self.model.A(self.T), self.x.T) +
np.dot(self.model.B(self.T), self.controls.T)).T
self.CoP = np.dot(self.model.D(self.T), self.x.T)
#first future step
self.f_future = np.array([[
x_opt[3 * self.N], x_opt[3 * self.N + self.future_steps],
x_opt[3 * self.N + 2 * self.future_steps]
]])
#change the current (every 8 samples except the first step) foot position
if np.mod(i, self.samples_per_step) == self.samples_per_step - 2:
self.f_current = self.f_future
#rotate the footstep bounds
self.current_rotation = np.array(
[[np.cos(self.f_current[0, 2]),
np.sin(self.f_current[0, 2])],
[-np.sin(self.f_current[0, 2]),
np.cos(self.f_current[0, 2])]])
#swap footstep constraints
tmp1, tmp2 = self.feet_ubounds[1], self.feet_lbounds[1]
self.feet_ubounds[1], self.feet_lbounds[1] = self.feet_ubounds[
3], self.feet_lbounds[3]
self.feet_ubounds[3], self.feet_lbounds[3] = tmp1, tmp2
#return all the variables of interest
return [self.x, self.f_current, self.CoP, self.controls]
class ClassicGenerator(BaseGenerator):
"""
Reimplementation of current state-of-the-art pattern
generator for HRP-2 of CNRS-LAAS, Toulouse.
Solve QPs for position and orientation of CoM and feet
independently of each other in each timestep.
First solve for orientations, then solve for the postions.
"""
def __init__(self, N=16, T=0.1, T_step=0.8, fsm_state='D', fsm_sl=1):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(ClassicGenerator, self).__init__(N, T, T_step, fsm_state, fsm_sl)
# TODO for speed up one can define members of BaseGenerator as
# direct views of QP data structures according to walking report
# Maybe do that later!
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x^T * H(w0) * x + x^T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # number of working set recalculations
self.options = Options()
self.options.setToMPC()
self.options.printLevel = PrintLevel.LOW
# FOR ORIENTATIONS
# define dimensions
self.ori_nv = 2 * self.N
self.ori_nc = (self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq)
# setup problem
self.ori_dofs = numpy.zeros(self.ori_nv)
self.ori_qp = SQProblem(self.ori_nv, self.ori_nc)
self.ori_qp.setOptions(self.options) # load NMPC options
self.ori_H = numpy.zeros((self.ori_nv, self.ori_nv))
self.ori_A = numpy.zeros((self.ori_nc, self.ori_nv))
self.ori_g = numpy.zeros((self.ori_nv, ))
self.ori_lb = -numpy.ones((self.ori_nv, )) * 1e+08
self.ori_ub = numpy.ones((self.ori_nv, )) * 1e+08
self.ori_lbA = -numpy.ones((self.ori_nc, )) * 1e+08
self.ori_ubA = numpy.ones((self.ori_nc, )) * 1e+08
self._ori_qp_is_initialized = False
# save computation time and working set recalculations
self.ori_qp_nwsr = 0.0
self.ori_qp_cputime = 0.0
# FOR POSITIONS
# define dimensions
self.pos_nv = 2 * (self.N + self.nf)
self.pos_nc = (self.nc_cop + self.nc_foot_position +
self.nc_fchange_eq)
# setup problem
self.pos_dofs = numpy.zeros(self.pos_nv)
self.pos_qp = SQProblem(self.pos_nv, self.pos_nc)
self.pos_qp.setOptions(self.options)
self.pos_H = numpy.zeros((self.pos_nv, self.pos_nv))
self.pos_A = numpy.zeros((self.pos_nc, self.pos_nv))
self.pos_g = numpy.zeros((self.pos_nv, ))
self.pos_lb = -numpy.ones((self.pos_nv, )) * 1e+08
self.pos_ub = numpy.ones((self.pos_nv, )) * 1e+08
self.pos_lbA = -numpy.ones((self.pos_nc, )) * 1e+08
self.pos_ubA = numpy.ones((self.pos_nc, )) * 1e+08
self._pos_qp_is_initialized = False
# save computation time and working set recalculations
self.pos_qp_nwsr = 0.0
self.pos_qp_cputime = 0.0
# dummy matrices
self._ori_Q = numpy.zeros((2 * self.N, 2 * self.N))
self._ori_p = numpy.zeros((2 * self.N, ))
self._pos_Q = numpy.zeros((self.N + self.nf, self.N + self.nf))
self._pos_p = numpy.zeros((self.N + self.nf, ))
# add additional keys that should be saved
self._data_keys.append('ori_qp_nwsr')
self._data_keys.append('ori_qp_cputime')
self._data_keys.append('pos_qp_nwsr')
self._data_keys.append('pos_qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
def solve(self):
""" Process and solve problem, s.t. pattern generator data is consistent """
self._preprocess_solution()
self._solve_qp()
self._postprocess_solution()
def _preprocess_solution(self):
""" Update matrices and get them into the QP data structures """
# rename for convenience
N = self.N
nf = self.nf
# ORIENTATIONS
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._ori_qp_is_initialized:
# ori_dofs = ( dddF_k_qR )
# ( dddF_k_qL )
self.ori_dofs[:N] = self.dddF_k_qR
self.ori_dofs[N:] = self.dddF_k_qL
# TODO guess initial active set
# this requires changes to the python interface
# define QP matrices
# H = ( Q_k_q )
self._update_ori_Q() # updates values in _Q
self.ori_H[:, :] = self._ori_Q
# g = ( p_k_q )
self._update_ori_p() # updates values in _p
self.ori_g[:] = self._ori_p
# ORIENTATION LINEAR CONSTRAINTS
# velocity constraints on support foot to freeze movement
a = 0
b = self.nc_fvel_eq
self.ori_A[a:b] = self.A_fvel_eq
self.ori_lbA[a:b] = self.B_fvel_eq
self.ori_ubA[a:b] = self.B_fvel_eq
# box constraints for maximum orientation change
a = self.nc_fvel_eq
b = self.nc_fvel_eq + self.nc_fpos_ineq
self.ori_A[a:b] = self.A_fpos_ineq
self.ori_lbA[a:b] = self.lbB_fpos_ineq
self.ori_ubA[a:b] = self.ubB_fpos_ineq
# box constraints for maximum angular velocity
a = self.nc_fvel_eq + self.nc_fpos_ineq
b = self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq
self.ori_A[a:b] = self.A_fvel_ineq
self.ori_lbA[a:b] = self.lbB_fvel_ineq
self.ori_ubA[a:b] = self.ubB_fvel_ineq
# ORIENTATION BOX CONSTRAINTS
#self.ori_lb [...] = 0.0
#self.ori_ub [...] = 0.0
# POSITIONS
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._pos_qp_is_initialized:
# pos_dofs = ( dddC_kp1_x )
# ( F_k_x )
# ( dddC_kp1_y )
# ( F_k_y )
self.pos_dofs[0:0 + N] = self.dddC_k_x
self.pos_dofs[0 + N:0 + N + nf] = self.F_k_x
a = N + nf
self.pos_dofs[a:a + N] = self.dddC_k_y
self.pos_dofs[a + N:a + N + nf] = self.F_k_y
# TODO guess initial active set
# this requires changes to the python interface
# define QP matrices
# H = ( Q_k 0 )
# ( 0 Q_k )
self._update_pos_Q() # updates values in _Q
self.pos_H[:N + nf, :N + nf] = self._pos_Q
self.pos_H[-N - nf:, -N - nf:] = self._pos_Q
# g = ( p_k_x )
# ( p_k_y )
self._update_pos_p('x') # updates values in _p
self.pos_g[:N + nf] = self._pos_p
self._update_pos_p('y') # updates values in _p
self.pos_g[-N - nf:] = self._pos_p
# lbA <= A x <= ubA
# ( ) <= ( Acop ) <= ( Bcop )
# (eqBfoot) <= ( eqAfoot ) <= ( eqBfoot )
# ( ) <= ( Afoot ) <= ( Bfoot )
# CoP constraints
a = 0
b = self.nc_cop
self.pos_A[a:b] = self.Acop
self.pos_lbA[a:b] = self.lbBcop
self.pos_ubA[a:b] = self.ubBcop
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.pos_A[a:b] = self.Afoot
self.pos_lbA[a:b] = self.lbBfoot
self.pos_ubA[a:b] = self.ubBfoot
#foot equality constraints
a = self.nc_cop + self.nc_foot_position
b = self.nc_cop + self.nc_foot_position + self.nc_fchange_eq
self.pos_A[a:b] = self.eqAfoot
self.pos_lbA[a:b] = self.eqBfoot
self.pos_ubA[a:b] = self.eqBfoot
# NOTE: they stay plus minus infinity, i.e. 1e+08
#self.pos_lb [...] = 0.0
#self.pos_ub [...] = 0.0
def _update_ori_Q(self):
'''
Update Hessian block Q according to walking report
min a/2 || dC_kp1_q_ref - dF_k_q ||_2^2
Q = ( QR 0 )
( 0 QL )
QR = ( a * Pvu^T * E_FR^T * E_FR * Pvu )
'''
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
Pvu = self.Pvu
# assemble Hessian matrix
# QR = ( a * Pvu^T * E_FR^T * E_FR * Pvu )
a = 0
b = N
c = 0
d = N
QR = self._ori_Q[a:b, c:d]
QR[...] = alpha * Pvu.transpose().dot(
E_FR.transpose()).dot(E_FR).dot(Pvu)
# QL = ( a * Pvu^T * E_FL^T * E_FL * Pvu )
# Q = ( * , * )
# ( * ,[*])
a = N
b = 2 * N
c = N
d = 2 * N
QL = self._ori_Q[a:b, c:d]
QL[...] = alpha * Pvu.transpose().dot(
E_FL.transpose()).dot(E_FL).dot(Pvu)
def _update_ori_p(self):
"""
Update pass gradient block p according to walking report
min a/2 || dC_kp1_q_ref - dF_k_q ||_2^2
p = ( pR )
( pL )
pR = ( a * Pvu^T * E_FR^T * (E_FR * Pvs * f_k_qR - dC_kp1_q_ref[N/2] )
"""
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
f_k_qR = self.f_k_qR
f_k_qL = self.f_k_qL
Pvs = self.Pvs
Pvu = self.Pvu
dC_kp1_q_ref = self.dC_kp1_q_ref
# pR = ( a * Pvu^T * E_FL^T * (E_FL * Pvs * f_k_qL + dC_kp1_q_ref) )
# p = ([*]) =
# ( * )
a = 0
b = N
self._ori_p[a:b] = \
alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR.dot(Pvs).dot(f_k_qR) - dC_kp1_q_ref)
# p = ( * ) =
# ([*])
a = N
b = 2 * N
self._ori_p[a:b] = \
alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL.dot(Pvs).dot(f_k_qL) - dC_kp1_q_ref)
def _update_pos_Q(self):
'''
Update Hessian block Q according to walking report
Q = ( a*Pvu*Pvu + b*Ppu*E*T*E*Ppu + c*Pzu*Pzu + d*I, -c*Pzu*V_kp1 )
( -c*Pzu*V_kp1, c*V_kp1*V_kp1 )
'''
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# cast matrices for convenience
Ppu = numpy.asmatrix(self.Ppu)
Pvu = numpy.asmatrix(self.Pvu)
Pzu = numpy.asmatrix(self.Pzu)
V_kp1 = numpy.asmatrix(self.V_kp1)
# Q = ([*], * ) = a*Pvu*Pvu + b*Ppu*E*E*Ppu + c*Pzu*Pzu + d*I
# ( * , * )
a = 0
b = N
c = 0
d = N
self._pos_Q[a:b,c:d] = alpha * Pvu.transpose() * Pvu \
+ gamma * Pzu.transpose() * Pzu \
+ delta * numpy.eye(N)
# Q = ( * ,[*])
# ( * , * )
a = 0
b = N
c = N
d = N + nf
self._pos_Q[a:b, c:d] = -gamma * Pzu.transpose() * V_kp1
# Q = ( * , * ) = ( * , [*] )^T
# ( [*], * ) ( * , * )
dummy = self._pos_Q[a:b, c:d]
a = N
b = N + nf
c = 0
d = N
self._pos_Q[a:b, c:d] = dummy.transpose()
# Q = ( * , * )
# ( * ,[*])
a = N
b = N + nf
c = N
d = N + nf
self._pos_Q[a:b, c:d] = gamma * V_kp1.transpose() * V_kp1
def _update_pos_p(self, case=None):
"""
Update pass gradient block p according to walking report
p = ( a*Pvu*(Pvs*ck - Refk+1) + b*Ppu*E*(E*Pps*cx - Refk+1) + c*Pzu*(Pzs*ck - vk+1*fk )
( -c*Vk+1*(Pzs*ck - vk+1*fk )
"""
if case == 'x':
f_k = self.f_k_x
c_k = utility.cast_array_as_matrix(self.c_k_x)
dC_kp1_ref = utility.cast_array_as_matrix(self.dC_kp1_x_ref)
elif case == 'y':
f_k = self.f_k_y
c_k = utility.cast_array_as_matrix(self.c_k_y)
dC_kp1_ref = utility.cast_array_as_matrix(self.dC_kp1_y_ref)
else:
err_str = 'Please use either case "x" or "y" for this routine'
raise AttributeError(err_str)
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
# matrices
v_kp1 = utility.cast_array_as_matrix(self.v_kp1)
Pvs = numpy.asmatrix(self.Pvs)
Pvu = numpy.asmatrix(self.Pvu)
Pps = numpy.asmatrix(self.Pps)
Ppu = numpy.asmatrix(self.Ppu)
Pzs = numpy.asmatrix(self.Pzs)
Pzu = numpy.asmatrix(self.Pzu)
V_kp1 = numpy.asmatrix(self.V_kp1)
# p = ([*]) =
# ( * )
a = 0
b = N
self._pos_p[a:b] = (
alpha * Pvu.transpose() * (Pvs * c_k - dC_kp1_ref) +
gamma * Pzu.transpose() * (Pzs * c_k - v_kp1 * f_k)
#+ b*Ppu.transpose() * E.transpose() * E * Ppu \
).ravel()
# p = ( * ) =
# ([*])
a = N
b = N + nf
self._pos_p[a:b] = (-gamma * V_kp1.transpose() *
(Pzs * c_k - v_kp1 * f_k)).ravel()
# WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
# A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
# details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with qpsolvers. If not, see <http://www.gnu.org/licenses/>.
from numpy import array, hstack, ones, vstack, zeros
from qpoases import PyOptions as Options
from qpoases import PyPrintLevel as PrintLevel
from qpoases import PyQProblem as QProblem
from qpoases import PyQProblemB as QProblemB
from qpoases import PyReturnValue as ReturnValue
__infty = 1e10
options = Options()
options.printLevel = PrintLevel.NONE
def qpoases_solve_qp(P,
q,
G=None,
h=None,
A=None,
b=None,
initvals=None,
max_wsr=1000):
"""
Solve a Quadratic Program defined as:
minimize
class SolverLPQpOases(solver_LP_abstract.SolverLPAbstract):
"""
Linear Program solver:
minimize c' x
subject to Alb <= A x <= Aub
lb <= x <= ub
"""
def __init__(self,
name,
maxIter=1000,
maxTime=100.0,
useWarmStart=True,
verb=0):
solver_LP_abstract.SolverLPAbstract.__init__(self, name, maxIter,
maxTime, useWarmStart,
verb)
self._hessian_regularization = DEFAULT_HESSIAN_REGULARIZATION
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._n = -1
self._m_con = -1
self._qpOasesSolver = None
def set_option(self, key, value):
if (key == 'hessian_regularization'):
self.set_hessian_regularization(value)
return True
return False
def set_hessian_regularization(self, reg):
assert reg > 0, "Hessian regularization must be positive"
self._hessian_regularization = reg
if (self._n > 0):
self._Hess = self._hessian_regularization * np.identity(self._n)
''' Solve the linear program
minimize c' x
subject to Alb <= A_in x <= Aub
A_eq x = b
lb <= x <= ub
Return a tuple containing:
status flag
primal solution
dual solution
'''
def solve(self,
c,
lb,
ub,
A_in=None,
Alb=None,
Aub=None,
A_eq=None,
b=None):
start = time.time()
n = c.shape[0]
m_con = 0
if ((A_in is not None) and (A_eq is not None)):
m_con = A_in.shape[0] + A_eq.shape[0]
if (m_con != self._m_con or n != self._n):
self._A_con = np.empty((m_con, n))
self._lb_con = np.empty((m_con))
self._ub_con = np.empty((m_con))
m_in = A_in.shape[0]
self._A_con[:m_in, :] = A_in
self._lb_con[:m_in] = Alb
self._ub_con[:m_in] = Aub
self._A_con[m_in:, :] = A_eq
self._lb_con[m_in:] = b
self._ub_con[m_in:] = b
elif (A_in is not None):
m_con = A_in.shape[0]
if (m_con != self._m_con or n != self._n):
self._A_con = np.empty((m_con, n))
self._lb_con = np.empty((m_con))
self._ub_con = np.empty((m_con))
self._A_con[:, :] = A_in
self._lb_con[:] = Alb
self._ub_con[:] = Aub
elif (A_eq is not None):
m_con = A_eq.shape[0]
if (m_con != self._m_con or n != self._n):
self._A_con = np.empty((m_con, n))
self._lb_con = np.empty((m_con))
self._ub_con = np.empty((m_con))
self._A_con[:, :] = A_eq
self._lb_con[:] = b
self._ub_con[:] = b
else:
m_con = 0
if (m_con != self._m_con or n != self._n):
self._A_con = np.empty((m_con, n))
self._lb_con = np.empty((m_con))
self._ub_con = np.empty((m_con))
if (n != self._n or m_con != self._m_con):
self._qpOasesSolver = SQProblem(n, m_con)
#, HessianType.SEMIDEF);
self._qpOasesSolver.setOptions(self._options)
self._Hess = self._hessian_regularization * np.identity(n)
self._x = np.zeros(n)
self._y = np.zeros(n + m_con)
self._n = n
self._m_con = m_con
self._initialized = False
maxActiveSetIter = np.array([self._maxIter])
maxComputationTime = np.array(self._maxTime)
if (not self._useWarmStart or not self._initialized):
self._imode = self._qpOasesSolver.init(self._Hess, c, self._A_con,
lb, ub, self._lb_con,
self._ub_con,
maxActiveSetIter,
maxComputationTime)
else:
self._imode = self._qpOasesSolver.hotstart(
self._Hess, c, self._A_con, lb, ub, self._lb_con, self._ub_con,
maxActiveSetIter, maxComputationTime)
self._lpTime = maxComputationTime
self._iter = 1 + maxActiveSetIter[0]
if (self._imode == 0):
self._initialized = True
self._lpStatus = solver_LP_abstract.LP_status.OPTIMAL
self._qpOasesSolver.getPrimalSolution(self._x)
self._qpOasesSolver.getDualSolution(self._y)
self.checkConstraints(self._x, lb, ub, A_in, Alb, Aub, A_eq, b)
else:
if (self._imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY
or self._imode == PyReturnValue.INIT_FAILED_INFEASIBILITY):
self._lpStatus = solver_LP_abstract.LP_status.INFEASIBLE
elif (self._imode == PyReturnValue.MAX_NWSR_REACHED):
self._lpStatus = solver_LP_abstract.LP_status.MAX_ITER_REACHED
else:
self._lpStatus = solver_LP_abstract.LP_status.ERROR
self.reset()
self._x = np.zeros(n)
self._y = np.zeros(n + m_con)
if (self._verb > 0):
print "[%s] ERROR Qp oases %s" % (
self._name, qpOasesSolverMsg(self._imode))
if (self._lpTime >= self._maxTime):
if (self._verb > 0):
print "[%s] Max time reached %f after %d iters" % (
self._name, self._lpTime, self._iter)
self._imode = 9
self._computationTime = time.time() - start
return (self._lpStatus, self._x, self._y)
