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)
# Setting up SQProblem object and solution analyser. example = SQProblem(2, 1) analyser = SolutionAnalysis() # Solve first QP ... nWSR = 10 example.init(H, g, A, lb, ub, lbA, ubA, nWSR) # ... and analyse it. maxKKTviolation = np.zeros(1) analyser.getMaxKKTviolation(example, maxKKTviolation) print("maxKKTviolation: %e\n"%maxKKTviolation) # Solve second QP ... nWSR = 10; example.hotstart(H_new, g_new, A_new, lb_new, ub_new, lbA_new, ubA_new, nWSR) # ... and analyse it. analyser.getMaxKKTviolation(example, maxKKTviolation) print("maxKKTviolation: %e\n"%maxKKTviolation) # ------------ 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 )
class hotqpOASESSolverWrapper(SolverWrapper): """`qpOASES` solver wrapper with hot-start. This wrapper takes advantage of the warm-start capability of the qpOASES quadratic programming solver. It uses two different qp solvers. One to solve for maximized controllable sets and one to solve for minimized controllable sets. The wrapper selects which solver to use by looking at the optimization direction. This solver wrapper also scale data before invoking `qpOASES`. If the logger "toppra" is set to debug level, qpoases solvers are initialized with PrintLevel.HIGH. Otherwise, these are initialized with PrintLevel.NONE Currently only support Canonical Linear Constraints. Parameters ---------- constraint_list: :class:`.Constraint` [] The constraints the robot is subjected to. path: :class:`.Interpolator` The geometric path. path_discretization: array The discretized path positions. disable_check: bool, optional Disable check for solution validity. Improve speed by about 20% but entails the possibility that failure is not reported correctly. scaling_solverwrapper: bool, optional If is True, try to scale the data of each optimization before running. """ def __init__( self, constraint_list, path, path_discretization, disable_check=False, # scaling_solverwrapper=True, ): if not qpoases_FOUND: SolverNotFound("toppra is unable to find any installation of qpoases!") super(hotqpOASESSolverWrapper, self).__init__( constraint_list, path, path_discretization ) self._disable_check = disable_check # First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u self.nC = 2 # number of Constraints. 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] # qpOASES coefficient arrays # l <= var <= h # lA <= A var <= hA self._A = np.zeros((self.nC, self.nV)) self._lA = -np.ones(self.nC) * QPOASES_INFTY self._hA = np.ones(self.nC) * QPOASES_INFTY self._l = -np.ones(2) * QPOASES_INFTY self._h = np.ones(2) * QPOASES_INFTY 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 close_solver(self): self.solver_minimizing = None self.solver_maximizing = None def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): assert i <= self.N and 0 <= i # solve the scaled optimization problem # min 0.5 y^T scale H scale y + g^T scale y # s.t lA <= A scale y <= hA # l <= scale y <= h self._l[:] = -QPOASES_INFTY self._h[:] = QPOASES_INFTY if x_min is not None: self._l[1] = max(self._l[1], x_min) if x_max is not None: self._h[1] = min(self._h[1], x_max) if i < self.N: delta = self.get_deltas()[i] if x_next_min is not None: self._A[0] = [-2 * delta, -1] self._hA[0] = -x_next_min else: self._A[0] = [0, 0] self._hA[0] = QPOASES_INFTY self._lA[0] = -QPOASES_INFTY if x_next_max is not None: self._A[1] = [2 * delta, 1] self._hA[1] = x_next_max else: self._A[1] = [0, 0] self._hA[1] = QPOASES_INFTY self._lA[1] = -QPOASES_INFTY cur_index = 2 for j in range(len(self.constraints)): a, b, c, F, v, ubound, xbound = self.params[j] if a is not None: if self.constraints[j].identical: nC_ = F.shape[0] self._A[cur_index : cur_index + nC_, 0] = F.dot(a[i]) self._A[cur_index : cur_index + nC_, 1] = F.dot(b[i]) self._hA[cur_index : cur_index + nC_] = v - F.dot(c[i]) self._lA[cur_index : cur_index + nC_] = -QPOASES_INFTY else: nC_ = F[i].shape[0] self._A[cur_index : cur_index + nC_, 0] = F[i].dot(a[i]) self._A[cur_index : cur_index + nC_, 1] = F[i].dot(b[i]) self._hA[cur_index : cur_index + nC_] = v[i] - F[i].dot(c[i]) self._lA[cur_index : cur_index + nC_] = -QPOASES_INFTY cur_index = cur_index + nC_ if ubound is not None: self._l[0] = max(self._l[0], ubound[i, 0]) self._h[0] = min(self._h[0], ubound[i, 1]) if xbound is not None: self._l[1] = max(self._l[1], xbound[i, 0]) self._h[1] = min(self._h[1], xbound[i, 1]) # if x_min == x_max, do not solve the 2D linear program, instead, do a line search if abs(x_min - x_max) < TINY and H is None and self.get_no_vars() == 2: logger.debug("x_min ({:f}) equals x_max ({:f})".format(x_min, x_max)) u_min = -QPOASES_INFTY u_max = QPOASES_INFTY for i in range(self._A.shape[0]): if self._A[i, 0] > 0: u_max = min( u_max, (self._hA[i] - self._A[i, 1] * x_min) / self._A[i, 0] ) elif self._A[i, 0] < 0: u_min = max( u_min, (self._hA[i] - self._A[i, 1] * x_min) / self._A[i, 0] ) if (u_min - u_max) / abs(u_max) > SMALL: # problem infeasible logger.debug( "u_min > u_max by {:f}. Might not be critical. " "Returning failure.".format(u_min - u_max) ) return np.array([np.nan, np.nan]) if g[0] < 0: return np.array([u_max, x_min + 2 * u_max * delta]) else: return np.array([u_min, x_min + 2 * u_min * delta]) if H is None: H = ( np.ones((self.get_no_vars(), self.get_no_vars())) * 1e-18 ) # regularization, very important ratio_col1 = 1 / ( np.sum(np.abs(self._A[2:, 0])) + 1e-5 ) # the maximum possible value for both ratios is 100000 ratio_col2 = 1 / (np.sum(np.abs(self._A[2:, 1])) + 1e-5) variable_scales = np.array([ratio_col1, ratio_col2]) # variable_scales = np.array([5000.0, 2000.0]) variable_scales_mat = np.diag(variable_scales) if logger.isEnabledFor(logging.DEBUG): logger.debug( "min ratio col 1 {:f}, col 2 {:f}".format(ratio_col1, ratio_col2) ) # ratio scaling self._A = self._A.dot(variable_scales_mat) self._l = self._l / variable_scales self._h = self._h / variable_scales self._g = g * variable_scales self._H = variable_scales_mat.dot(H).dot(variable_scales_mat) # rows scaling row_magnitude = np.sum(np.abs(self._A), axis=1) row_scaling_mat = np.diag((row_magnitude + 1) ** (-1)) self._A = np.dot(row_scaling_mat, self._A) self._lA = np.dot(row_scaling_mat, self._lA) self._hA = np.dot(row_scaling_mat, self._hA) return_value, var = self._solve_optimization(i) if return_value == ReturnValue.SUCCESSFUL_RETURN: if logger.isEnabledFor(logging.DEBUG): logger.debug("optimal value: {:}".format(var)) if self._disable_check: return var * variable_scales # Check for constraint feasibility success = ( np.all(self._l <= var + TINY) and np.all(var <= self._h + TINY) and np.all(np.dot(self._A, var) <= self._hA + TINY) and np.all(np.dot(self._A, var) >= self._lA - TINY) ) if not success: # import ipdb; ipdb.set_trace() logger.fatal( "Hotstart fails but qpOASES does not report correctly. \n " "var: {:}, lower_bound: {:}, higher_bound{:}".format( var, self._l, self._h ) ) # TODO: Investigate why this happen and fix the # relevant code (in qpOASES wrapper) else: return var * variable_scales else: logger.debug("Optimization fails. qpOASES error code: %d.", return_value) if ( np.all(0 <= self._hA) and np.all(0 >= self._lA) and np.all(0 <= self._h) and np.all(0 >= self._l) ): logger.fatal( "(0, 0) satisfies all constraints => error due to numerical errors.", self._A, self._lA, self._hA, self._l, self._h, ) else: logger.debug("(0, 0) does not satisfy all constraints.") return_value = np.empty(self.get_no_vars()) return_value[:] = np.nan return return_value def _solve_optimization(self, i): var = np.zeros(self.nV) if self._g[1] > 0: # Choose solver_minimizing if abs(self.solver_minimizing_recent_index - i) > 1: logger.debug("solver_minimizing [init]") return_value = self.solver_minimizing.init( self._H, self._g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000]), ) else: if logger.isEnabledFor(logging.DEBUG): logger.debug("solver_minimizing [hotstart]") return_value = self.solver_minimizing.hotstart( self._H, self._g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000]), ) self.solver_minimizing_recent_index = i self.solver_minimizing.getPrimalSolution(var) else: # Choose solver_maximizing if abs(self.solver_maximizing_recent_index - i) > 1: if logger.isEnabledFor(logging.DEBUG): logger.debug("solver_maximizing [init]") return_value = self.solver_maximizing.init( self._H, self._g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000]), ) else: if logger.isEnabledFor(logging.DEBUG): logger.debug("solver_maximizing [hotstart]") return_value = self.solver_maximizing.hotstart( self._H, self._g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000]), ) self.solver_maximizing_recent_index = i self.solver_maximizing.getPrimalSolution(var) return return_value, var
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)
g=np.array(-A_coupl.T*b_coupl).T[0] lb=-100.0*np.ones(A_coupl.shape[1]) ub= 100.0*np.ones(A_coupl.shape[1]) return_qp = 0 if (FIRST_QP==True): options = Options() options.printLevel = PrintLevel.NONE qpb = SQProblem(A_coupl.shape[1],A_.shape[0]) qpb.setOptions(options) qpb.init(H,g,A_,lb,ub,lb_A,ub_A,np.array([QPmaxIt])) sol=np.zeros(A_coupl.shape[1]) FIRST_QP=False print "INITIALISATION OF THE QP" else: return_qp = qpb.hotstart(H,g,A_,lb,ub,lb_A,ub_A,np.array([QPmaxIt])) qpb.getPrimalSolution(sol) log_return_qp.append(return_qp) solution = np.matrix(sol).T else: #Using least square equality constrained : ********************* solution = LSEC(A_coupl,b_coupl,A_,np.matrix(lb_A).T) qddot = solution[-p.robot.nv:] #qddot
# 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 ... nWSR = np.array([10]) example.hotstart(H_new, g_new, A_new, lb_new, ub_new, lbA_new, ubA_new, nWSR) # ... and analyse it. analyser.getKktViolation(example, maxStat, maxFeas, maxCmpl) print("maxStat: %e, maxFeas:%e, maxCmpl: %e\n"%(maxStat, maxFeas, maxCmpl)) # ------------ 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 )
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)
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
class hotqpOASESSolverWrapper(SolverWrapper): """A solver wrapper using `qpOASES`. This wrapper takes advantage of the warm-start capability of qpOASES quadratic programming solver by using two different solvers. One to solve for maximized controllable sets and one to solve for minimized controllable sets. If the logger "toppra" is set to debug level, qpoases solvers are initialized with PrintLevel.HIGH. Otherwise, these are initialized with PrintLevel.NONE Parameters ---------- constraint_list: list of :class:`.Constraint` The constraints the robot is subjected to. path: :class:`.Interpolator` The geometric path. path_discretization: array The discretized path positions. disable_check: bool, optional Disable check for solution validity. Improve speed by about 20% but entails the possibility that failure is not reported correctly. """ def __init__(self, constraint_list, path, path_discretization, disable_check=False): assert qpoases_FOUND, "toppra is unable to find any installation of qpoases!" super(hotqpOASESSolverWrapper, self).__init__(constraint_list, path, path_discretization) self._disable_check = disable_check # 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) * INF self._hA = np.ones(self.nC) * INF self._l = -np.ones(2) * INF self._h = np.ones(2) * INF def setup_solver(self): option = Options() if logger.getEffectiveLevel() == logging.DEBUG: # option.printLevel = PrintLevel.HIGH option.printLevel = PrintLevel.NONE 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 close_solver(self): self.solver_up = None self.solver_down = None def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max): # NOTE: qpOASES solve QPs of the following form: # min 0.5 y^T H y + g^T y # s.t lA <= Ay <= hA # l <= y <= h assert i <= self.N and 0 <= i self._l[:] = -INF self._h[:] = INF if x_min is not None: self._l[1] = max(self._l[1], x_min) if x_max is not None: self._h[1] = min(self._h[1], x_max) if i < self.N: delta = self.get_deltas()[i] if x_next_min is not None: self._A[0] = [-2 * delta, -1] self._hA[0] = -x_next_min else: self._A[0] = [0, 0] self._hA[0] = INF if x_next_max is not None: self._A[1] = [2 * delta, 1] self._hA[1] = x_next_max else: self._A[1] = [0, 0] self._hA[1] = INF cur_index = 2 for j in range(len(self.constraints)): a, b, c, F, v, ubound, xbound = self.params[j] if a is not None: if self.constraints[j].identical: nC_ = F.shape[0] self._A[cur_index:cur_index + nC_, 0] = F.dot(a[i]) self._A[cur_index:cur_index + nC_, 1] = F.dot(b[i]) self._hA[cur_index:cur_index + nC_] = v - F.dot(c[i]) self._lA[cur_index:cur_index + nC_] = -INF else: nC_ = F[i].shape[0] self._A[cur_index:cur_index + nC_, 0] = F[i].dot(a[i]) self._A[cur_index:cur_index + nC_, 1] = F[i].dot(b[i]) self._hA[cur_index:cur_index + nC_] = v[i] - F[i].dot(c[i]) self._lA[cur_index:cur_index + nC_] = -INF cur_index = cur_index + nC_ if ubound is not None: self._l[0] = max(self._l[0], ubound[i, 0]) self._h[0] = min(self._h[0], ubound[i, 1]) if xbound is not None: self._l[1] = max(self._l[1], xbound[i, 0]) self._h[1] = min(self._h[1], xbound[i, 1]) if H is None: H = np.zeros((self.get_no_vars(), self.get_no_vars())) # Select what solver to use if g[1] > 0: # Choose solver_up if abs(self.solver_up_recent_index - i) > 1: if logger.isEnabledFor(logging.DEBUG): logger.debug("Choose solver [up] - init") res = self.solver_up.init(H, g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000])) else: if logger.isEnabledFor(logging.DEBUG): logger.debug("Choose solver [up] - hotstart") res = self.solver_up.hotstart(H, g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000])) self.solver_up_recent_index = i else: # Choose solver_down if abs(self.solver_down_recent_index - i) > 1: if logger.isEnabledFor(logging.DEBUG): logger.debug("Choose solver [down] - init") res = self.solver_down.init(H, g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000])) else: if logger.isEnabledFor(logging.DEBUG): logger.debug("Choose solver [down] - hotstart") res = self.solver_down.hotstart(H, g, self._A, self._l, self._h, self._lA, self._hA, np.array([1000])) self.solver_down_recent_index = i if res == ReturnValue.SUCCESSFUL_RETURN: var = np.zeros(self.nV) if g[1] > 0: self.solver_up.getPrimalSolution(var) else: self.solver_down.getPrimalSolution(var) if self._disable_check: return var # Check for constraint feasibility success = (np.all(self._l <= var + eps) and np.all(var <= self._h + eps) and np.all(np.dot(self._A, var) <= self._hA + eps) and np.all(np.dot(self._A, var) >= self._lA - eps)) if not success: # import ipdb; ipdb.set_trace() logger.fatal( "Hotstart fails but qpOASES does not report correctly. \n " "var: {:}, lower_bound: {:}, higher_bound{:}".format( var, self._l, self._h)) # TODO: Investigate why this happen and fix the # relevant code (in qpOASES wrapper) else: return var res = np.empty(self.get_no_vars()) res[:] = np.nan return res
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) # 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
err_CP_l ])).reshape(ConstraintsSize,) ubA = np.array(np.vstack([ -err_lf-dJdq_lf, -err_rf-dJdq_rf, -err_rh-dJdq_rh[:3], err_CP_u ])).reshape(ConstraintsSize,) lb = -1000*np.ones(robot.nv) ub = 1000*np.ones(robot.nv) if i==0: qp.init(H, g, A, lb, ub, lbA, ubA, np.array([100])) else: qp.hotstart(H, g, A, lb, ub, lbA, ubA, np.array([100])) sol = np.zeros(robot.nv) qp.getPrimalSolution(sol) a = np.matrix(sol).T se3.rnea(robot.model,robot.data,q,v,a) tau_joints = robot.data.tau[6:] ##Â configuration integration v += np.matrix(a*dt) robot.increment(q,v*dt) ## Display new configuration robot.display(q)
class StaggeredProjections (object): """ Algorithm described in "Staggered projections for frictional contact in multibody systems". """ USE_DIAGONAL_GENERATORS = True; name = ""; # name of this instance n = 0; # number of variables k = 0; # number of contact points D = []; # friction cost matrix ET = []; # friction inequality matrix mu_alpha = []; # friction inequality upper-bound vector N = []; # contact cost matrix bounds_contact = []; # bounds on the problem variables bounds_friction = []; # bounds on the problem variables H_contact = []; # Hessian H = G^T*G g_contact = []; # Product dD = d^T*D H_friction = []; # Hessian H = G^T*G g_friction = []; # Product dD = d^T*D alpha = [] # contact impulses beta = []; # friciton impulses f0 = []; # initial guess for the friction impulses projected in joint space 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 iter_contact = 0; iter_friction = 0; qpTime_contact = 0.0; qpTime_friction = 0.0; t = 0; # number of times the method simulate has been called meanComputationTime = 0.0; maxComputationTime = 0.0; maxIterations = 0; meanIterations = 0.0; initialized_contact = False; # true if solver has been initialized initialized_friction = False; # true if solver has been initialized solverContact = []; solverFriction = []; options = []; # qp oases solver's options epsilon = np.sqrt(np.finfo(float).eps); INEQ_VIOLATION_THR = 1e-4; def __init__ (self, n, mu, accuracy=1e-4, maxIter=30, verb=0): self.name = "StagProj"; self.n = n; self.mu = mu; self.iter = 0; self.accuracy = accuracy; self.maxIter = maxIter; self.verb = verb; self.options = Options(); 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 = True; self.changeContactsNumber(0); self.S_T = np.zeros((self.n+6,self.n)); self.S_T[6:6+self.n,:] = np.identity(self.n); self.t = 0; self.meanComputationTime = 0.0; self.maxComputationTime = 0.0; self.meanIterations = 0.0; self.maxIterations = 0.0; return; 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 simulate(self, v, M, h, tau, dt, contactList, dJv_list, maxIter=None, maxTime=100.0): start = time.time(); if(maxIter==None): maxIter = self.maxIter; self.maxTime = maxTime; self.iter = 0; self.min_res = 1e10; # minimum residual self.rel_err = 1e10; # relative error self.contactList = contactList; self.dJv_list = dJv_list; self.dt = dt; ''' If necessary resize the solvers ''' if(self.k!=len(contactList)): self.changeContactsNumber(len(contactList)); ''' Compute next momentum assuming no contacts ''' self.M_inv = np.linalg.inv(M); self.M_vp = np.dot(M,v) - dt*h; self.M_vp[6:] += dt*tau; self.v_p = np.dot(self.M_inv, self.M_vp); if(self.k==0): return (self.v_p, np.zeros(0)); self.f_old = np.copy(self.f0); while (self.rel_err>self.accuracy and self.iter<maxIter): self.N_alpha = self.compute_contact_projection(-self.M_vp-self.f_old); if(self.mu<1e-3): break; self.f_new = self.compute_friction_projection(-self.M_vp-self.N_alpha); self.rel_err = self.compute_relative_error(); self.residual = self.compute_residual(); if(self.residual < self.min_res): self.min_res = self.residual; self.f_star = np.copy(self.f_new); # cache best solution self.iter += 1; self.f_old = np.copy(self.f_new); if(self.verb>1): print "Iter %d err %f resid %f alpha %.3f beta %.3f" % (self.iter, self.rel_err, self.residual, np.linalg.norm(self.alpha)/dt, np.linalg.norm(self.beta)/dt); if(self.verb>0 and self.rel_err>self.accuracy): print "[Stag-Proj] Algorithm did not converge", print "Iter %d err %f resid %f alpha %.3f beta %.3f" % (self.iter, self.rel_err, self.residual, np.linalg.norm(self.alpha)/dt, np.linalg.norm(self.beta)/dt); # time.sleep(2); if(self.mu>1e-3): self.f0 = np.copy(self.f_star); self.N_alpha = self.compute_contact_projection(-self.M_vp-self.f0); v_next = self.v_p + np.dot(self.M_inv, self.N_alpha+self.f0); ddx = np.zeros(self.k); fz = np.zeros(self.k); v_xy = np.zeros(2*self.k); f_xy = np.zeros(2*self.k); for i in range(self.k): ''' compute normal contact accelerations and contact forces to check KKT conditions ''' ddx[i] = np.dot(self.contactList[i][2,:], v_next) + dt*self.dJv_list[i][2]; fz[i] = self.alpha[i]/dt; if(self.verb>0): ''' compute tangent velocities and forces ''' v_xy[2*i:2*i+2] = np.dot(self.contactList[i][0:2,:], v_next); f_xy[2*i:2*i+2] = np.dot(self.T[0:2,:], self.beta[4*i:4*i+4])/dt; slide_acc = 1e-2; if(np.linalg.norm(v_xy[2*i:2*i+2])>slide_acc and fz[i]>10.0): print "Contact point %d is sliding, fxy/fz=(%.2f,%.2f), fz=%.1f, v=(%.2f,%.2f,%.2f)"%(i,f_xy[2*i]/fz[i],f_xy[2*i+1]/fz[i],fz[i], v_xy[2*i],v_xy[2*i+1],ddx[i]); # time.sleep(1); ''' compute contact forces ''' self.contact_forces[3*i:3*i+3] = np.dot(self.T, self.beta[4*i:4*i+4])/dt; self.contact_forces[3*i+2] = self.alpha[i]/dt; self.v_xy = v_xy; self.f_xy = f_xy; if(np.dot(ddx,fz)>self.accuracy): print 'ERROR STAG-PROJ: ddx*fz = %f' % np.dot(ddx,fz); print (' ddx: ', ddx); print (' fz: ', fz); self.computationTime = time.time()-start; self.t += 1; self.meanComputationTime = ((self.t-1)*self.meanComputationTime + self.computationTime)/self.t; self.meanIterations = ((self.t-1)*self.meanIterations + self.iter)/self.t; if(self.maxComputationTime < self.computationTime): self.maxComputationTime = self.computationTime; if(self.maxIterations < self.iter): self.maxIterations = self.iter; return (v_next, self.contact_forces); def compute_contact_projection(self, z): if(self.iter==0): for i in range(0,self.k): self.N[:,i] = self.contactList[i][2,:].T; self.dJv[i] = self.dJv_list[i][2]; self.H_contact = np.dot(self.N.transpose(), np.dot(self.M_inv, self.N)) + 1e-10*np.identity(self.k); self.g_contact = -np.dot(self.N.transpose(), np.dot(self.M_inv, z)) + self.dt*self.dJv; A = np.zeros((0,self.k)); lbA = np.zeros(0); ubA = np.zeros(0); maxActiveSetIter = np.array([2*self.k]); maxComputationTime = np.array(self.maxTime); if(self.initialized_contact==False): imode = self.solverContact.init(self.H_contact, self.g_contact, A, self.lb_contact, self.ub_contact, lbA, ubA, maxActiveSetIter, maxComputationTime); if(imode!=PyReturnValue.INIT_FAILED_INFEASIBILITY): self.initialized_contact = True; else: imode = self.solverContact.hotstart(self.H_contact, self.g_contact, A, self.lb_contact, self.ub_contact, lbA, ubA, maxActiveSetIter, maxComputationTime); self.qpTime_contact += maxComputationTime; self.iter_contact += maxActiveSetIter[0]; if(imode==0 or imode==PyReturnValue.MAX_NWSR_REACHED): self.solverContact.getPrimalSolution(self.alpha); self.print_qp_oases_error_message(imode, "Contact solver"); return np.dot(self.N, self.alpha); def compute_friction_projection(self, z): if(self.iter==0): self.T_dJ_dq = np.zeros(4*self.k); for i in range(0,self.k): self.D[:,4*i:4*i+4] = np.dot(self.contactList[i].T, self.T); self.T_dJ_dq[4*i:4*i+4] += self.dt * np.dot(self.T[0:2,:].T, self.dJv_list[i][0:2]); self.H_friction = np.dot(self.D.transpose(), np.dot(self.M_inv, self.D)); ''' compute constraint vector (upper bound) ''' self.mu_alpha = self.mu * self.alpha; self.g_friction = -np.dot(self.D.transpose(), np.dot(self.M_inv, z)) + self.T_dJ_dq; maxActiveSetIter = np.array([8*self.k]); maxComputationTime = np.array(self.maxTime); if(self.initialized_friction==False): imode = self.solverFriction.init(self.H_friction, self.g_friction, self.ET, self.lb_friction, self.ub_friction, self.lbA_friction, self.mu_alpha, maxActiveSetIter, maxComputationTime); if(imode==0): self.initialized_friction = True; else: imode = self.solverFriction.hotstart(self.H_friction, self.g_friction, self.ET, self.lb_friction, self.ub_friction, self.lbA_friction, self.mu_alpha, maxActiveSetIter, maxComputationTime); self.qpTime_friction += maxComputationTime; self.iter_friction += maxActiveSetIter[0]; if(imode==0 or imode==PyReturnValue.MAX_NWSR_REACHED): self.solverFriction.getPrimalSolution(self.beta); self.print_qp_oases_error_message(imode, "Friction solver"); return np.dot(self.D, self.beta); def compute_relative_error(self): f_diff = self.f_new - self.f_old; num = np.dot(f_diff.T, np.dot(self.M_inv, f_diff)); den = np.dot(self.f_old.T, np.dot(self.M_inv, self.f_old)); if(den!=0.0): return num/den; return 1e10; def compute_residual(self): res = 0.0; tmp = self.v_p + np.dot(self.M_inv, self.N_alpha + self.f_new); for i in range(self.k): res += abs(self.alpha[i]*np.dot(self.N[:,i].T, tmp)); return res; def reset(self): self.initialized_contact = False; self.initialized_friction = False; def print_qp_oases_error_message(self, imode, solver_name): if(imode!=0 and imode!=63 and self.verb>0): if(imode==PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY): print "[%s] ERROR Qp oases HOTSTART_STOPPED_INFEASIBILITY" % solver_name; # 60 elif(imode==PyReturnValue.MAX_NWSR_REACHED): print "[%s] ERROR Qp oases RET_MAX_NWSR_REACHED" % solver_name; # 63 elif(imode==PyReturnValue.STEPDIRECTION_FAILED_CHOLESKY): print "[%s] ERROR Qp oases STEPDIRECTION_FAILED_CHOLESKY" % solver_name; # 68 elif(imode==PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED): print "[%s] ERROR Qp oases HOTSTART_FAILED_AS_QP_NOT_INITIALISED" % solver_name; # 53 elif(imode==PyReturnValue.INIT_FAILED_INFEASIBILITY): print "[%s] ERROR Qp oases INIT_FAILED_INFEASIBILITY" % solver_name; # 37 # RET_INIT_FAILED_HOTSTART = 36 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);
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 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 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
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