def __init__(self,
nv,
nc,
objective,
constraints,
bounds,
num_iter=3,
num_wsr=100,
verbose=False):
"""Initialize the SQP."""
self.nv = nv
self.nc = nc
self.num_iter = num_iter
self.num_wsr = num_wsr
self.objective = objective
self.constraints = constraints
self.bounds = bounds
self.qp = qpoases.PySQProblem(nv, nc)
if not verbose:
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
self.qp.setOptions(options)
self.qp_initialized = False
def __init__(self, dim_a, dim_b):
self.qpProblem = qpoases.PySQProblem(dim_a, dim_b)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
self.qpProblem.setOptions(options)
self.xdot_full = np.zeros(dim_a)
self.started = False
def __init__(self, dim_a, dim_b):
"""
:param dim_a: number of joint constraints + number of soft constraints
:type int
:param dim_b: number of hard constraints + number of soft constraints
:type int
"""
self.qpProblem = qpoases.PySQProblem(dim_a, dim_b)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
self.qpProblem.setOptions(options)
self.xdot_full = np.zeros(dim_a)
self.started = False
def _iterate(self, q0, dq0, pr, u, N, pc):
ni = self.model.ni
# Create the QP, which we'll solve sequentially.
# num vars, num constraints (note that constraints only refer to matrix
# constraints rather than bounds)
# num constraints = N obstacle constraints and ni*N joint acceleration
# constraints
num_var = ni * N
num_constraints = 3 * N + ni * N
qp = qpoases.PySQProblem(num_var, num_constraints)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qp.setOptions(options)
# Initial opt problem.
H, g, A_obs, lbA_obs = self._lookahead(q0, pr, u, N, pc)
ubA_obs = np.infty * np.ones_like(lbA_obs)
lb, ub = self._calc_vel_limits(u, ni, N)
A_acc, lbA_acc, ubA_acc = self._calc_acc_limits(u, dq0, ni, N)
A = np.vstack((A_obs, A_acc))
lbA = np.concatenate((lbA_obs, lbA_acc))
ubA = np.concatenate((ubA_obs, ubA_acc))
ret = qp.init(H, g, A, lb, ub, lbA, ubA, np.array([NUM_WSR]))
delta = np.zeros(ni * N)
qp.getPrimalSolution(delta)
u = u + delta
# Remaining sequence is hotstarted from the first.
for i in range(NUM_ITER - 1):
H, g, A_obs, lbA_obs = self._lookahead(q0, pr, u, N, pc)
lb, ub = self._calc_vel_limits(u, ni, N)
A_acc, lbA_acc, ubA_acc = self._calc_acc_limits(u, dq0, ni, N)
A = np.vstack((A_obs, A_acc))
lbA = np.concatenate((lbA_obs, lbA_acc))
ubA = np.concatenate((ubA_obs, ubA_acc))
qp.hotstart(H, g, A, lb, ub, lbA, ubA, np.array([NUM_WSR]))
qp.getPrimalSolution(delta)
u = u + delta
return u