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 changeContactsNumber(self, k):
if(self.verb>1):
print "[%s] Changing number of contacts from %d to %d" % (self.name, self.k, k);
self.k = k;
self.iter = 0;
self.initialized_contact = False;
self.initialized_friction = False;
if(k>0):
self.solverContact = SQProblem(self.k, 0);
self.solverFriction = SQProblem(4*self.k, self.k);
self.solverContact.setOptions(self.options);
self.solverFriction.setOptions(self.options);
self.contact_forces = np.zeros(self.k*3);
self.f0 = np.zeros(self.n+6); # reset initial guess
self.bounds_contact = self.k*[(0.0,1e9)];
self.bounds_friction = (4*self.k)*[(0.0,1e9)];
self.beta = np.zeros(4*self.k);
self.N = np.zeros((self.n+6, self.k));
self.dJv = np.zeros(self.k);
self.lb_contact = np.array([ b[0] for b in self.bounds_contact]);
self.ub_contact = np.array([ b[1] for b in self.bounds_contact]);
self.alpha = np.zeros(self.k);
''' compute constraint matrix '''
self.ET = np.zeros((self.k, 4*self.k));
for i in range(0,self.k):
self.ET[i,4*i:4*i+4] = 1;
self.D = np.zeros((self.n+6, 4*self.k));
''' compute tangent directions matrix '''
self.T = np.zeros((3, 4)); # tangent directions
if(self.USE_DIAGONAL_GENERATORS):
tmp = 1.0/sqrt(2.0);
self.T[:,0] = np.array([+tmp, +tmp, 0]);
self.T[:,1] = np.array([+tmp, -tmp, 0]);
self.T[:,2] = np.array([-tmp, +tmp, 0]);
self.T[:,3] = np.array([-tmp, -tmp, 0]);
else:
self.T[:,0] = np.array([+1, 0, 0]);
self.T[:,1] = np.array([-1, 0, 0]);
self.T[:,2] = np.array([ 0, +1, 0]);
self.T[:,3] = np.array([ 0, -1, 0]);
self.lb_friction = np.array([ b[0] for b in self.bounds_friction]);
self.ub_friction = np.array([ b[1] for b in self.bounds_friction]);
self.lbA_friction = -1e100*np.ones(self.k);
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 __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 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 __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 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 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 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, n, m_in, maxIter=1000, verb=1):
self.name = name
self.iter = 0
self.maxIter = maxIter
self.verb = verb
self.m_in = m_in
self.n = n
self.iter = 0
self.qpOasesSolver = SQProblem(self.n, self.m_in)
#, HessianType.SEMIDEF);
self.options = Options()
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 > 4):
self.options.printLevel = PrintLevel.DEBUG_ITER
print("set high print level")
self.options.enableRegularisation = True
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
self.Hess = np.identity(self.n)
self.grad = np.zeros(self.n)
self.x_lb = np.array(self.n * [
-1e10,
])
self.x_ub = np.array(self.n * [
1e10,
])
self.B = np.zeros((self.m_in, self.n))
self.b_ub = np.zeros(self.m_in) + 1e10
self.b_lb = np.zeros(self.m_in) - 1e10
self.x = np.zeros(self.n)
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)
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.
example = SQProblem(2, 1)
analyser = SolutionAnalysis()
# Solve first QP ...
nWSR = np.array([10])
example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
# ... and analyse it.
maxStat = np.zeros(1)
maxFeas = np.zeros(1)
maxCmpl = np.zeros(1)
analyser.getKktViolation(example, maxStat, maxFeas, maxCmpl)
print("maxStat: %e, maxFeas:%e, maxCmpl: %e\n"%(maxStat, maxFeas, maxCmpl))
# Solve second QP ...
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 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)
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 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 __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 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.3
# Initial joint values.
q0_init = q0 = 0.01
q1_init = q1 = -0.02
# Joint target.
q_des1 = 0.45
q_des2 = 0.78
# Slack limits.
e1_max = 1000
e2_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-2
p = 10
q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1
error1 = p * (q_des1 - q_eef)
error2 = p * (q_des2 - q_eef)
vel_init = 0
nWSR = np.array([100])
# Dynamic goal weights
w3 = (error2 / (error1 + error2))**2 # goal 1 weight
w4 = (error1 / (error1 + error2))**2 # goal 2 weight
# Acceleration
a0_const = (v0_max - a0_max) / v0_max
a1_const = (v1_max - a1_max) / v1_max
example = SQProblem(4, 6)
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, 1.0, 0.0, 1.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
]).reshape((6, 4))
g = np.array([0.0, 0.0, 0.0, 0.0])
lb = np.array([-v0_max, -v1_max, -e1_max, -e2_max])
ub = np.array([v0_max, v1_max, e1_max, e2_max])
lbA = np.array(
[error1, error2, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max])
ubA = np.array(
[error1, error2, (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(4)
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
i = 0
# Stopping conditions
limit1 = abs(error1)
limit2 = abs(error2)
lim = min([limit1, limit2])
diff1 = abs(error1)
diff2 = abs(error2)
diff = min([diff1, diff2])
# 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)
r_max = np.array(q_des1)
r_min = np.array(q_des2)
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 (diff1 > 0.00005 or diff2 > 0.0005) and limit1 > precision and limit2 > precision and i < 400:
while diff > 0.00005 and lim > precision and i < 400:
# Solve QP.
i += 1
nWSR = np.array([100])
lbA[0] = error1
lbA[1] = error2
lbA[2] = -q0_max - q0
lbA[3] = -q1_max - q1
lbA[4] = a0_const * Opt[0] - a0_max
lbA[5] = a1_const * Opt[1] - a1_max
ubA[0] = error1
ubA[1] = error2
ubA[2] = q0_max - q0
ubA[3] = q1_max - q1
ubA[4] = a0_const * Opt[0] + a0_max
ubA[5] = 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
# Update limits
example.getPrimalSolution(Opt)
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)
# Update weights
w3 = (error2 / (error1 + error2))**2
w4 = (error1 / (error1 + error2))**2
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))
# Stopping conditions
limit1 = abs(error1)
limit2 = abs(error2)
lim = min([limit1, limit2])
diff1 = abs(error1 - old_error1)
diff2 = abs(error2 - old_error2)
diff = min([diff1, diff2])
#print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, w3= %g, w4= %g, q0= %g q1= %g \n" % (
#Opt[0], Opt[1], Opt[2], Opt[3], q_eef, w3, w4, q0, q1)
print w3, w4
# 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))
r_max = np.hstack((r_max, q_des1))
r_min = np.hstack((r_min, q_des2))
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_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=r_max,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_1',
line=dict(dash='dot'))
t_g2 = go.Scatter(y=r_min,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_2',
line=dict(dash='dot', width=4))
data = [t_eef, t_veef, t_g1, t_g2]
layout = dict(
title="Initial position EEF = %g. Goals: goal_1 = %g, goal_2 = %g"
% (q_eef_init, q_des1, q_des2),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
),
font=dict(size=18),
yaxis=dict(title='Position / Velocity'),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/dynamic_weights_eef.html',
image='png',
image_filename='dynamic_2')
data = [t_p0, t_v0]
layout = dict(title="Initial position q0 =%g." % (q0_init),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
),
yaxis=dict(title='Position / Velocity',
gridwidth=3,
range=[-0.15, 0.05]),
font=dict(size=18))
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/dynamic_weights_q0.html',
image='png',
image_filename='dynamic_3')
data = [t_p1, t_v1]
layout = dict(
title="Initial position q1 = %g." % (q1_init),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
),
yaxis=dict(title='Position / Velocity',
gridwidth=3,
range=[-0.15, 0.05]),
font=dict(size=18),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/dynamic_weights_q1.html',
image='png',
image_filename='dynamic_4')
print "\n i = %g \n" % i
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 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 __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_assembly_of_dofs(self):
# define initial values
comx = [0.00949035, 0.0, 0.0]
comy = [0.095, 0.0, 0.0]
comz = 0.814
footx = 0.00949035
footy = 0.095
footq = 0.0
# pattern generator preparation
gen = NMPCGenerator()
# set reference velocities to zero
gen.set_velocity_reference([0.2, 0.0, -0.2])
gen.set_security_margin(0.04, 0.04)
# set initial values
gen.set_initial_values(comx,
comy,
comz,
footx,
footy,
footq,
foot='left')
# hack generator
gen.nc = 0
# setup problem
gen.dofs = numpy.zeros(gen.nv)
gen.qp = SQProblem(gen.nv, gen.nc)
# load NMPC options
#gen.qp.setOptions(gen.options)
gen.H = numpy.eye(gen.nv, gen.nv)
gen.A = numpy.zeros((gen.nc, gen.nv))
gen.g = numpy.zeros((gen.nv, ))
gen.lb = -numpy.ones((gen.nv, )) * 1e+08
gen.ub = numpy.ones((gen.nv, )) * 1e+08
gen.lbA = -numpy.ones((gen.nc, )) * 1e+08
gen.ubA = numpy.ones((gen.nc, )) * 1e+08
gen._qp_is_initialized = False
# renaming for convenience
N = gen.N
nf = gen.nf
# define specified input
gen.dddC_k_x[...] = 1.0
gen.F_k_x[...] = 2.0
gen.dddC_k_y[...] = 3.0
gen.F_k_y[...] = 4.0
gen.dddF_k_qR[...] = 5.0
gen.dddF_k_qL[...] = 6.0
dddC_k_x = gen.dddC_k_x.copy()
F_k_x = gen.F_k_x.copy()
dddC_k_y = gen.dddC_k_y.copy()
F_k_y = gen.F_k_y.copy()
dddF_k_qR = gen.dddF_k_qR.copy()
dddF_k_qL = gen.dddF_k_qL.copy()
# do an assembly of the problem
gen._preprocess_solution()
# check if dofs are assembled correctly
assert_allclose(gen.dofs[0:0 + N], gen.dddC_k_x)
assert_allclose(gen.dofs[0 + N:0 + N + nf], gen.F_k_x)
a = N + nf
assert_allclose(gen.dofs[a:a + N], gen.dddC_k_y)
assert_allclose(gen.dofs[a + N:a + N + nf], gen.F_k_y)
a = 2 * (N + nf)
assert_allclose(gen.dofs[a:a + N], gen.dddF_k_qR)
assert_allclose(gen.dofs[-N:], gen.dddF_k_qL)
# reset initilization
gen.H[...] = numpy.eye(gen.nv, gen.nv)
gen.A[...] = numpy.zeros((gen.nc, gen.nv))
gen.g[...] = numpy.zeros((gen.nv, ))
gen.lb[...] = -numpy.ones((gen.nv, )) * 1e+08
gen.ub[...] = numpy.ones((gen.nv, )) * 1e+08
gen.lbA[...] = -numpy.ones((gen.nc, )) * 1e+08
gen.ubA[...] = numpy.ones((gen.nc, )) * 1e+08
# calculate solution which shall be equal to zero
#gen._solve_qp()
#assert_allclose(gen.dofs, 0.0, atol=ATOL, rtol=RTOL)
# hack solution
gen.dofs[0:0 + N] = 1.0
gen.dofs[0 + N:0 + N + nf] = 2.0
a = N + nf
gen.dofs[a:a + N] = 3.0
gen.dofs[a + N:a + N + nf] = 4.0
a = 2 * (N + nf)
gen.dofs[a:a + N] = 5.0
gen.dofs[-N:] = 6.0
# adjust reference values
dddC_k_x[...] = 2.0
F_k_x[...] = 4.0
dddC_k_y[...] = 6.0
F_k_y[...] = 8.0
dddF_k_qR[...] = 10.0
dddF_k_qL[...] = 12.0
# add increment to
gen._postprocess_solution()
# check if dofs are assembled correctly
assert_allclose(gen.dddC_k_x, dddC_k_x)
assert_allclose(gen.F_k_x, F_k_x)
a = N + nf
assert_allclose(gen.dddC_k_y, dddC_k_y)
assert_allclose(gen.F_k_y, F_k_y)
a = 2 * (N + nf)
assert_allclose(gen.dddF_k_qR, dddF_k_qR)
assert_allclose(gen.dddF_k_qL, dddF_k_qL)
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-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 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
