def setup_solver(self): option = Options() if logger.getEffectiveLevel() == logging.DEBUG: option.printLevel = PrintLevel.HIGH else: option.printLevel = PrintLevel.NONE self.solver_up = SQProblem(self.nV, self.nC) self.solver_up.setOptions(option) self.solver_down = SQProblem(self.nV, self.nC) self.solver_down.setOptions(option) self.solver_up_recent_index = -2 self.solver_down_recent_index = -2
def changeContactsNumber(self, k): if(self.verb>1): print "[%s] Changing number of contacts from %d to %d" % (self.name, self.k, k); self.k = k; self.iter = 0; self.initialized_contact = False; self.initialized_friction = False; if(k>0): self.solverContact = SQProblem(self.k, 0); self.solverFriction = SQProblem(4*self.k, self.k); self.solverContact.setOptions(self.options); self.solverFriction.setOptions(self.options); self.contact_forces = np.zeros(self.k*3); self.f0 = np.zeros(self.n+6); # reset initial guess self.bounds_contact = self.k*[(0.0,1e9)]; self.bounds_friction = (4*self.k)*[(0.0,1e9)]; self.beta = np.zeros(4*self.k); self.N = np.zeros((self.n+6, self.k)); self.dJv = np.zeros(self.k); self.lb_contact = np.array([ b[0] for b in self.bounds_contact]); self.ub_contact = np.array([ b[1] for b in self.bounds_contact]); self.alpha = np.zeros(self.k); ''' compute constraint matrix ''' self.ET = np.zeros((self.k, 4*self.k)); for i in range(0,self.k): self.ET[i,4*i:4*i+4] = 1; self.D = np.zeros((self.n+6, 4*self.k)); ''' compute tangent directions matrix ''' self.T = np.zeros((3, 4)); # tangent directions if(self.USE_DIAGONAL_GENERATORS): tmp = 1.0/sqrt(2.0); self.T[:,0] = np.array([+tmp, +tmp, 0]); self.T[:,1] = np.array([+tmp, -tmp, 0]); self.T[:,2] = np.array([-tmp, +tmp, 0]); self.T[:,3] = np.array([-tmp, -tmp, 0]); else: self.T[:,0] = np.array([+1, 0, 0]); self.T[:,1] = np.array([-1, 0, 0]); self.T[:,2] = np.array([ 0, +1, 0]); self.T[:,3] = np.array([ 0, -1, 0]); self.lb_friction = np.array([ b[0] for b in self.bounds_friction]); self.ub_friction = np.array([ b[1] for b in self.bounds_friction]); self.lbA_friction = -1e100*np.ones(self.k);
def set_contacts(self, contact_points, contact_normals, mu, regularization=1e-5): # compute matrix A, which maps the force generator coefficients into the centroidal wrench (self.A, self.G4) = compute_centroidal_cone_generators(contact_points, contact_normals, mu) # since the size of the problem may have changed we need to recreate the solver and all the problem matrices/vectors self.n = contact_points.shape[0] * 4 + 1 self.qpOasesSolver = SQProblem(self.n, self.m_in) #, HessianType.SEMIDEF); self.qpOasesSolver.setOptions(self.options) self.Hess = INITIAL_HESSIAN_REGULARIZATION * np.identity(self.n) self.grad = np.ones(self.n) * regularization self.grad[-1] = 1.0 self.constrMat = np.zeros((self.m_in, self.n)) self.constrMat[:, :-1] = self.A self.constrMat[:3, -1] = self.mass * self.v self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v) self.lb = np.zeros(self.n) self.lb[-1] = -1e100 self.ub = np.array(self.n * [ 1e100, ]) self.x = np.zeros(self.n) self.y = np.zeros(self.n + self.m_in) self.initialized = False
def __initSolver(self, arg_dim, num_in): self.solver = SQProblem(arg_dim, num_in) # , HessianType.POSDEF SEMIDEF self.options = Options() self.options.setToReliable() self.solver.setOptions(self.options)
def changeInequalityNumber(self, m_in): # print "[%s] Changing number of inequality constraints from %d to %d" % (self.name, self.m_in, m_in); if (m_in == self.m_in): return self.m_in = m_in self.iter = 0 self.qpOasesSolver = SQProblem(self.n, m_in) #, HessianType.POSDEF SEMIDEF self.options = Options() self.options.setToReliable() if (self.verb <= 0): self.options.printLevel = PrintLevel.NONE elif (self.verb == 1): self.options.printLevel = PrintLevel.LOW elif (self.verb == 2): self.options.printLevel = PrintLevel.MEDIUM elif (self.verb > 2): self.options.printLevel = PrintLevel.DEBUG_ITER print "set high print level" # self.options.enableRegularisation = False; # self.options.enableFlippingBounds = BooleanType.FALSE # self.options.initialStatusBounds = SubjectToStatus.INACTIVE # self.options.setToMPC(); # self.qpOasesSolver.printOptions(); self.qpOasesSolver.setOptions(self.options) self.initialized = False
def __init__(self, constraint_list, path, path_discretization): assert qpoases_FOUND, "toppra is unable to find any installation of qpoases!" super(qpOASESSolverWrapper, self).__init__( constraint_list, path, path_discretization ) # Currently only support Canonical Linear Constraint self.nC = 2 # First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u for i, constraint in enumerate(constraint_list): if constraint.get_constraint_type() != ConstraintType.CanonicalLinear: raise NotImplementedError a, b, c, F, v, _, _ = self.params[i] if a is not None: if constraint.identical: self.nC += F.shape[0] else: self.nC += F.shape[1] self._A = np.zeros((self.nC, self.nV)) self._lA = -np.ones(self.nC) self._hA = -np.ones(self.nC) option = Options() option.printLevel = PrintLevel.NONE self.solver = SQProblem(self.nV, self.nC) self.solver.setOptions(option)
def setup_solver(self): """Initiate two internal solvers for warm-start. """ option = Options() if logger.getEffectiveLevel() == logging.DEBUG: # option.printLevel = PrintLevel.HIGH option.printLevel = PrintLevel.NONE else: option.printLevel = PrintLevel.NONE self.solver_minimizing = SQProblem(self.nV, self.nC) self.solver_minimizing.setOptions(option) self.solver_maximizing = SQProblem(self.nV, self.nC) self.solver_maximizing.setOptions(option) self.solver_minimizing_recent_index = -2 self.solver_maximizing_recent_index = -2
def solveLeastSquare(A, b, lb=None, ub=None, A_in=None, lb_in=None, ub_in=None): ''' Solve the least square problem: minimize || A*x-b ||^2 subject to lb_in <= A_in*x <= ub_in lb <= x <= ub ''' n = A.shape[1] m_in = 0 if A_in is not None: m_in = A_in.shape[0] if lb_in is None: lb_in = np.array(m_in * [-1e99]) if ub_in is None: ub_in = np.array(m_in * [1e99]) if lb is None: lb = np.array(n * [-1e99]) if ub is None: ub = np.array(n * [1e99]) Hess = np.dot(A.transpose(), A) grad = -np.dot(A.transpose(), b) maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n]) maxComputationTime = np.array([600.0]) options = Options() options.printLevel = PrintLevel.LOW # NONE, LOW, MEDIUM options.enableRegularisation = True print('Gonna solve QP...') if m_in == 0: qpOasesSolver = QProblemB(n) # , HessianType.SEMIDEF); qpOasesSolver.setOptions(options) imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter, maxComputationTime) else: qpOasesSolver = SQProblem(n, m_in) # , HessianType.SEMIDEF); qpOasesSolver.setOptions(options) imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in, maxActiveSetIter, maxComputationTime) print('QP solved in %f seconds and %d iterations' % (maxComputationTime[0], maxActiveSetIter[0])) if imode != 0 and imode != 63: print("ERROR Qp oases %d " % (imode)) x_norm = np.zeros(n) # solution of the normalized problem qpOasesSolver.getPrimalSolution(x_norm) return x_norm
def solveLeastSquare(A, b, lb=None, ub=None, A_in=None, lb_in=None, ub_in=None): n = A.shape[1] m_in = 0 if (A_in != None): m_in = A_in.shape[0] if (lb_in == None): lb_in = np.array(m_in * [-1e99]) if (ub_in == None): ub_in = np.array(m_in * [1e99]) if (lb == None): lb = np.array(n * [-1e99]) if (ub == None): ub = np.array(n * [1e99]) Hess = np.dot(A.transpose(), A) grad = -np.dot(A.transpose(), b) maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n]) maxComputationTime = np.array([600.0]) options = Options() options.printLevel = PrintLevel.LOW #NONE, LOW, MEDIUM options.enableRegularisation = True print 'Gonna solve QP...' if (m_in == 0): qpOasesSolver = QProblemB(n) #, HessianType.SEMIDEF); qpOasesSolver.setOptions(options) imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter, maxComputationTime) else: qpOasesSolver = SQProblem(n, m_in) #, HessianType.SEMIDEF); qpOasesSolver.setOptions(options) imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in, maxActiveSetIter, maxComputationTime) print 'QP solved in %f seconds and %d iterations' % (maxComputationTime[0], maxActiveSetIter[0]) if (imode != 0 and imode != 63): print "ERROR Qp oases %d " % (imode) x_norm = np.zeros(n) # solution of the normalized problem qpOasesSolver.getPrimalSolution(x_norm) return x_norm
def __init__(self, name, n, m_in, maxIter=1000, verb=1): self.name = name self.iter = 0 self.maxIter = maxIter self.verb = verb self.m_in = m_in self.n = n self.iter = 0 self.qpOasesSolver = SQProblem(self.n, self.m_in) #, HessianType.SEMIDEF); self.options = Options() if (self.verb <= 1): self.options.printLevel = PrintLevel.NONE elif (self.verb == 2): self.options.printLevel = PrintLevel.LOW elif (self.verb == 3): self.options.printLevel = PrintLevel.MEDIUM elif (self.verb > 4): self.options.printLevel = PrintLevel.DEBUG_ITER print("set high print level") self.options.enableRegularisation = True self.qpOasesSolver.setOptions(self.options) self.initialized = False self.Hess = np.identity(self.n) self.grad = np.zeros(self.n) self.x_lb = np.array(self.n * [ -1e10, ]) self.x_ub = np.array(self.n * [ 1e10, ]) self.B = np.zeros((self.m_in, self.n)) self.b_ub = np.zeros(self.m_in) + 1e10 self.b_lb = np.zeros(self.m_in) - 1e10 self.x = np.zeros(self.n)
def solve(self, D, d, A, lbA, ubA, lb, ub, x0=None, maxIter=None, maxTime=100.0): if (self.NO_WARM_START): self.qpOasesSolver = SQProblem(self.n, self.m_in) self.qpOasesSolver.setOptions(self.options) self.initialized = False if (maxIter is None): maxIter = self.maxIter self.iter = 0 self.qpTime = 0.0 self.removeSoftInequalities = False self.setProblemData(D, d, A, lbA, ubA, lb, ub, x0) start = time.time() x = np.zeros(self.x0.shape) if (self.solver == 'slsqp'): (x, fx, self.iterationNumber, imode, smode) = fmin_slsqp(self.f_cost, self.x0, fprime=self.f_cost_grad, f_ieqcons=self.f_inequalities, fprime_ieqcons=self.f_inequalities_jac, bounds=self.bounds, iprint=self.verb, iter=maxIter, acc=self.accuracy, full_output=1) self.fx = fx x = np.array(x) ''' Exit modes are defined as follows : -1 : Gradient evaluation required (g & a) 0 : Optimization terminated successfully. 1 : Function evaluation required (f & c) 2 : More equality constraints than independent variables 3 : More than 3*n iterations in LSQ subproblem 4 : Inequality constraints incompatible 5 : Singular matrix E in LSQ subproblem 6 : Singular matrix C in LSQ subproblem 7 : Rank-deficient equality constraint subproblem HFTI 8 : Positive directional derivative for linesearch 9 : Iteration limit exceeded ''' if (self.verb > 0 and imode != 0 and imode != 9): #do not print error msg if iteration limit exceeded print "[%s] *** ERROR *** %s" % (self.name, smode) elif (self.solver == 'qpoases'): # ubA = np.array(self.m_in*[1e9]); # lb = np.array([ b[0] for b in self.bounds]); # ub = np.array([ b[1] for b in self.bounds]); # A = self.get_linear_inequality_matrix(); self.iter = 0 #total iters of qpoases # lbA = -self.get_linear_inequality_vector(); Hess = self.f_cost_hess(x) grad = self.f_cost_grad(x) self.fx = self.f_cost(x) maxActiveSetIter = np.array([maxIter - self.iter]) maxComputationTime = np.array(maxTime) if (self.initialized == False): imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb, self.ub, self.lbA, self.ubA, maxActiveSetIter, maxComputationTime) if (imode == 0): self.initialized = True else: imode = self.qpOasesSolver.hotstart(Hess, grad, self.A, self.lb, self.ub, self.lbA, self.ubA, maxActiveSetIter, maxComputationTime) if (imode == PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED): maxActiveSetIter = np.array([maxIter]) maxComputationTime = np.array(maxTime) imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb, self.ub, self.lbA, self.ubA, maxActiveSetIter, maxComputationTime) if (imode == 0): self.initialized = True self.qpTime += maxComputationTime self.iter = 1 + maxActiveSetIter[0] self.iterationNumber = self.iter ''' if the solution found is unfeasible check whether the initial guess is feasible ''' self.qpOasesSolver.getPrimalSolution(x) ineq_marg = self.f_inequalities(x) qpUnfeasible = False if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): qpUnfeasible = True if (x0 is not None): ineq_marg = self.f_inequalities(x0) if (not (ineq_marg < -self.INEQ_VIOLATION_THR).any()): if (self.verb > 0): print "[%s] Solution found is unfeasible but initial guess is feasible" % ( self.name) qpUnfeasible = False x = np.copy(x0) ''' if both the solution found and the initial guess are unfeasible remove the soft constraints ''' if (qpUnfeasible and len(self.softInequalityIndexes) > 0): # remove soft inequality constraints and try to solve again self.removeSoftInequalities = True maxActiveSetIter[0] = maxIter lbAsoft = np.copy(self.lbA) ubAsoft = np.copy(self.ubA) lbAsoft[self.softInequalityIndexes] = -1e100 ubAsoft[self.softInequalityIndexes] = 1e100 imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb, self.ub, lbAsoft, ubAsoft, maxActiveSetIter) self.qpOasesSolver.getPrimalSolution(x) ineq_marg = self.f_inequalities(x) ineq_marg[self.softInequalityIndexes] = 1.0 qpUnfeasible = False if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): ''' if the solution found is unfeasible check whether the initial guess is feasible ''' if (x0 is not None): x = np.copy(x0) ineq_marg = self.f_inequalities(x) ineq_marg[self.softInequalityIndexes] = 1.0 if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): print "[%s] WARNING Problem unfeasible even without soft constraints" % ( self.name), np.min(ineq_marg), imode qpUnfeasible = True elif (self.verb > 0): print "[%s] Initial guess is feasible for the relaxed problem" % ( self.name) else: print "[%s] No way to get a feasible solution (no initial guess)" % ( self.name), np.min(ineq_marg) elif (self.verb > 0): print "[%s] Solution found and initial guess are unfeasible, but relaxed problem is feasible" % ( self.name) if (qpUnfeasible): self.print_qp_oases_error_message(imode, self.name) if (self.verb > 1): activeIneq = np.count_nonzero(np.abs(ineq_marg) < 1e-3) print "[%s] Iter %d, active inequalities %d" % ( self.name, self.iter, activeIneq) # termination conditions if (self.iter >= maxIter): imode = 9 if (self.verb > 1): print "[%s] Max number of iterations reached %d" % ( self.name, self.iter) if (self.qpTime >= maxTime): print "[%s] Max time reached %f after %d iters" % ( self.name, self.qpTime, self.iter) imode = 9 elif (self.solver == 'sqpoases'): ubA = np.array(self.m_in * [1e99]) x_newton = np.zeros(x.shape) x = self.x0 A = self.get_linear_inequality_matrix() self.iter = 0 #total iters of qpoases self.outerIter = 0 # number of outer (Newton) iterations while True: # compute Newton step lb = np.array([b[0] for b in self.bounds]) ub = np.array([b[1] for b in self.bounds]) lb -= x ub -= x lbA = -np.dot(A, x) - self.get_linear_inequality_vector() # if(self.outerIter>0): # if((lbA>0.0).any()): # print "[%s] Iter %d lbA[%d]=%f"%(self.name,self.outerIter,np.argmax(lbA),np.max(lbA)); Hess = self.f_cost_hess(x) grad = self.f_cost_grad(x) self.fx = self.f_cost(x) maxActiveSetIter = np.array([maxIter - self.iter]) maxComputationTime = np.array(maxTime) if (self.initialized == False): imode = self.qpOasesSolver.init(Hess, grad, A, lb, ub, lbA, ubA, maxActiveSetIter, maxComputationTime) if (imode == 0): self.initialized = True else: imode = self.qpOasesSolver.hotstart( Hess, grad, A, lb, ub, lbA, ubA, maxActiveSetIter, maxComputationTime) self.qpTime += maxComputationTime maxTime -= maxComputationTime # count iterations self.iter += 1 + maxActiveSetIter[0] self.outerIter += 1 self.qpOasesSolver.getPrimalSolution(x_newton) ''' check feasibility of the constraints ''' x_new = x + x_newton ineq_marg = self.f_inequalities(x_new) if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): if (len(self.softInequalityIndexes) > 0 and self.outerIter == 1): ''' if constraints are unfeasible at first iteration remove soft inequalities ''' if (self.verb > 1): print '[%s] Remove soft constraints' % (self.name) self.removeSoftInequalities = True self.g[self.softInequalityIndexes] = 1e100 self.iter = 0 continue elif (len(self.softInequalityIndexes) > 0 and self.removeSoftInequalities == False): ''' if constraints are unfeasible at later iteration remove soft inequalities ''' if (self.verb >= 0): print '[%s] Remove soft constraints at iter %d' % ( self.name, self.outerIter) self.removeSoftInequalities = True self.g[self.softInequalityIndexes] = 1e100 continue else: if ((lbA > 0.0).any()): print "[%s] WARNING Problem unfeasible (even without soft constraints) %f" % ( self.name, np.max(lbA)), imode if (self.verb > 0): ''' qp failed for some reason (e.g. max iter) ''' print "[%s] WARNING imode %d ineq unfeasible iter %d: %f, max(lbA)=%f" % ( self.name, imode, self.outerIter, np.min(ineq_marg), np.max(lbA)) break ''' if constraints are unfeasible at later iterations exit ''' if (imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY): print "[%s] Outer iter %d QPoases says problem is unfeasible but constraints are satisfied: %f" % ( self.name, self.outerIter, np.min(ineq_marg)) ind = np.where(ineq_marg < 0.0)[0] self.g[ind] -= ineq_marg[ind] continue self.print_qp_oases_error_message(imode, self.name) # check for convergence newton_dec_squared = np.dot(x_newton, np.dot(Hess, x_newton)) if (0.5 * newton_dec_squared < self.accuracy): ineq_marg = self.f_inequalities(x) if ((ineq_marg >= -self.INEQ_VIOLATION_THR).all()): if (self.verb > 0): print "[%s] Optimization converged in %d steps" % ( self.name, self.iter) break elif (self.verb > 0): v = ineq_marg < -self.INEQ_VIOLATION_THR print( self.name, self.outerIter, "WARNING Solver converged but inequalities violated:", np.where(v), ineq_marg[v]) elif (self.verb > 1): print "[%s] Optimization did not converge yet, squared Newton decrement: %f" % ( self.name, newton_dec_squared) # line search (alpha, fc, gc, phi, old_fval, derphi) = line_search(self.f_cost, self.f_cost_grad, x, x_newton, grad, self.fx, c1=0.0001, c2=0.9) x_new = x + alpha * x_newton new_fx = self.f_cost(x_new) if (self.verb > 1): print "[%s] line search alpha = %f, fc %d, gc %d, old cost %f, new cost %f" % ( self.name, alpha, fc, gc, self.fx, new_fx) # Check that inequalities are still satisfied ineq_marg = self.f_inequalities(x_new) if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): if (self.verb > 1): print "[%s] WARNING some inequalities are violated with alpha=%f, gonna perform new line search." % ( self.name, alpha) k = 2.0 for i in range(100): alpha = min(k * alpha, 1.0) x_new = x + alpha * x_newton ineq_marg = self.f_inequalities(x_new) if ((ineq_marg >= -self.INEQ_VIOLATION_THR).all()): if (self.verb > 1): print "[%s] With alpha=%f the solution satisfies the inequalities." % ( self.name, alpha) break if (alpha == 1.0): print "[%s] ERROR With alpha=1 some inequalities are violated, error: %f" % ( self.name, np.min(ineq_marg)) break x = x_new if (self.verb > 1): ineq_marg = self.f_inequalities(x) activeIneq = np.count_nonzero(np.abs(ineq_marg) < 1e-3) nViolIneq = np.count_nonzero( ineq_marg < -self.INEQ_VIOLATION_THR) print "[%s] Outer iter %d, iter %d, active inequalities %d, violated inequalities %d" % ( self.name, self.outerIter, self.iter, activeIneq, nViolIneq) # termination conditions if (self.iter >= maxIter): if (self.verb > 1): print "[%s] Max number of iterations reached %d" % ( self.name, self.iter) imode = 9 break if (maxTime < 0.0): print "[%s] Max time reached %.4f s after %d out iters, %d iters, newtonDec %.6f removeSoftIneq" % ( self.name, self.qpTime, self.outerIter, self.iter, newton_dec_squared), self.removeSoftInequalities imode = 9 break self.iterationNumber = self.iter else: print '[%s] Solver type not recognized: %s' % (self.name, self.solver) return np.zeros(self.n) self.computationTime = time.time() - start ineq = self.f_inequalities(x) if (self.removeSoftInequalities): ineq[self.softInequalityIndexes] = 1.0 self.nViolatedInequalities = np.count_nonzero( ineq < -self.INEQ_VIOLATION_THR) self.nActiveInequalities = np.count_nonzero(ineq < 1e-3) self.imode = imode self.print_solution_info(x) self.finalize_solution(x) return (x, imode)
lb = np.array([ 0.5, -2.0 ]) ub = np.array([ 5.0, 2.0 ]) lbA = np.array([ -1.0 ]) ubA = np.array([ 2.0 ]) # Setup data of second QP. H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2)) A_new = np.array([ 1.0, 5.0 ]).reshape((2,1)) g_new = np.array([ 1.0, 1.5 ]) lb_new = np.array([ 0.0, -1.0 ]) ub_new = np.array([ 5.0, -0.5 ]) lbA_new = np.array([ -2.0 ]) ubA_new = np.array([ 1.0 ]) # Setting up SQProblem object and solution analyser. example = SQProblem(2, 1) analyser = SolutionAnalysis() # Solve first QP ... nWSR = np.array([10]) example.init(H, g, A, lb, ub, lbA, ubA, nWSR) # ... and analyse it. maxStat = np.zeros(1) maxFeas = np.zeros(1) maxCmpl = np.zeros(1) analyser.getKktViolation(example, maxStat, maxFeas, maxCmpl) print("maxStat: %e, maxFeas:%e, maxCmpl: %e\n"%(maxStat, maxFeas, maxCmpl)) # Solve second QP ...
def compute_max_deceleration(self, alpha, maxIter=None, maxTime=100.0): start = time.time() self.alpha = alpha if (self.NO_WARM_START): self.qpOasesSolver = SQProblem(self.n, self.m_in) self.qpOasesSolver.setOptions(self.options) self.initialized = False if (maxIter == None): maxIter = self.maxIter maxActiveSetIter = np.array([maxIter]) maxComputationTime = np.array(maxTime) self.constrUB[:6] = np.dot(self.b, alpha) + self.d self.constrLB[:6] = self.constrUB[:6] while (True): if (not self.initialized): self.imode = self.qpOasesSolver.init(self.Hess, self.grad, self.constrMat, self.lb, self.ub, self.constrLB, self.constrUB, maxActiveSetIter, maxComputationTime) else: self.imode = self.qpOasesSolver.hotstart( self.grad, self.lb, self.ub, self.constrLB, self.constrUB, maxActiveSetIter, maxComputationTime) if (self.imode == 0): self.initialized = True if (self.imode == 0 or self.imode == PyReturnValue.INIT_FAILED_INFEASIBILITY or self.imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY or self.Hess[0, 0] >= MAX_HESSIAN_REGULARIZATION): break self.initialized = False self.Hess *= 10.0 maxActiveSetIter = np.array([maxIter]) maxComputationTime = np.array(maxTime) if (self.verb > -1): print "[%s] WARNING %s. Increasing Hessian regularization to %f" % ( self.name, qpOasesSolverMsg(self.imode), self.Hess[0, 0]) self.qpTime = maxComputationTime self.iter = 1 + maxActiveSetIter[0] if (self.imode == 0): self.qpOasesSolver.getPrimalSolution(self.x) self.qpOasesSolver.getDualSolution(self.y) if ((self.x < self.lb - self.INEQ_VIOLATION_THR).any()): self.initialized = False raise ValueError("[%s] ERROR lower bound violated" % (self.name) + str(self.x) + str(self.lb)) if ((self.x > self.ub + self.INEQ_VIOLATION_THR).any()): self.initialized = False raise ValueError("[%s] ERROR upper bound violated" % (self.name) + str(self.x) + str(self.ub)) if ((np.dot(self.constrMat, self.x) > self.constrUB + self.INEQ_VIOLATION_THR).any()): self.initialized = False raise ValueError( "[%s] ERROR constraint upper bound violated " % (self.name) + str(np.min(np.dot(self.constrMat, self.x) - self.constrUB))) if ((np.dot(self.constrMat, self.x) < self.constrLB - self.INEQ_VIOLATION_THR).any()): self.initialized = False raise ValueError( "[%s] ERROR constraint lower bound violated " % (self.name) + str(np.max(np.dot(self.constrMat, self.x) - self.constrLB))) (dx, alpha_min, alpha_max) = self.compute_max_deceleration_derivative() else: self.initialized = False dx = 0.0 alpha_min = 0.0 alpha_max = 0.0 if (self.verb > 0): print "[%s] ERROR Qp oases %s" % (self.name, qpOasesSolverMsg(self.imode)) if (self.qpTime >= maxTime): if (self.verb > 0): print "[%s] Max time reached %f after %d iters" % ( self.name, self.qpTime, self.iter) self.imode = 9 self.computationTime = time.time() - start return (self.imode, self.x[-1], dx, alpha_min, alpha_max)
def solve(self, c, lb, ub, A_in=None, Alb=None, Aub=None, A_eq=None, b=None): start = time.time() n = c.shape[0] m_con = 0 if ((A_in is not None) and (A_eq is not None)): m_con = A_in.shape[0] + A_eq.shape[0] if (m_con != self._m_con or n != self._n): self._A_con = np.empty((m_con, n)) self._lb_con = np.empty((m_con)) self._ub_con = np.empty((m_con)) m_in = A_in.shape[0] self._A_con[:m_in, :] = A_in self._lb_con[:m_in] = Alb self._ub_con[:m_in] = Aub self._A_con[m_in:, :] = A_eq self._lb_con[m_in:] = b self._ub_con[m_in:] = b elif (A_in is not None): m_con = A_in.shape[0] if (m_con != self._m_con or n != self._n): self._A_con = np.empty((m_con, n)) self._lb_con = np.empty((m_con)) self._ub_con = np.empty((m_con)) self._A_con[:, :] = A_in self._lb_con[:] = Alb self._ub_con[:] = Aub elif (A_eq is not None): m_con = A_eq.shape[0] if (m_con != self._m_con or n != self._n): self._A_con = np.empty((m_con, n)) self._lb_con = np.empty((m_con)) self._ub_con = np.empty((m_con)) self._A_con[:, :] = A_eq self._lb_con[:] = b self._ub_con[:] = b else: m_con = 0 if (m_con != self._m_con or n != self._n): self._A_con = np.empty((m_con, n)) self._lb_con = np.empty((m_con)) self._ub_con = np.empty((m_con)) if (n != self._n or m_con != self._m_con): self._qpOasesSolver = SQProblem(n, m_con) #, HessianType.SEMIDEF); self._qpOasesSolver.setOptions(self._options) self._Hess = self._hessian_regularization * np.identity(n) self._x = np.zeros(n) self._y = np.zeros(n + m_con) self._n = n self._m_con = m_con self._initialized = False maxActiveSetIter = np.array([self._maxIter]) maxComputationTime = np.array(self._maxTime) if (not self._useWarmStart or not self._initialized): self._imode = self._qpOasesSolver.init(self._Hess, c, self._A_con, lb, ub, self._lb_con, self._ub_con, maxActiveSetIter, maxComputationTime) else: self._imode = self._qpOasesSolver.hotstart( self._Hess, c, self._A_con, lb, ub, self._lb_con, self._ub_con, maxActiveSetIter, maxComputationTime) self._lpTime = maxComputationTime self._iter = 1 + maxActiveSetIter[0] if (self._imode == 0): self._initialized = True self._lpStatus = solver_LP_abstract.LP_status.OPTIMAL self._qpOasesSolver.getPrimalSolution(self._x) self._qpOasesSolver.getDualSolution(self._y) self.checkConstraints(self._x, lb, ub, A_in, Alb, Aub, A_eq, b) else: if (self._imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY or self._imode == PyReturnValue.INIT_FAILED_INFEASIBILITY): self._lpStatus = solver_LP_abstract.LP_status.INFEASIBLE elif (self._imode == PyReturnValue.MAX_NWSR_REACHED): self._lpStatus = solver_LP_abstract.LP_status.MAX_ITER_REACHED else: self._lpStatus = solver_LP_abstract.LP_status.ERROR self.reset() self._x = np.zeros(n) self._y = np.zeros(n + m_con) if (self._verb > 0): print "[%s] ERROR Qp oases %s" % ( self._name, qpOasesSolverMsg(self._imode)) if (self._lpTime >= self._maxTime): if (self._verb > 0): print "[%s] Max time reached %f after %d iters" % ( self._name, self._lpTime, self._iter) self._imode = 9 self._computationTime = time.time() - start return (self._lpStatus, self._x, self._y)
def main(): rospy.init_node('controller_2dof', anonymous=True) rospy.Rate(10) # Setup data of QP. # Joint Weights. w1 = 1e-3 w2 = 1e-3 # Joint limits. q0_max = 0.05 q1_max = 0.15 # Links l1 = 0.1 l2 = 0.2 l3 = 0.25 # Initial joint values. q0_init = q0 = -0.03 q1_init = q1 = 0.0 # Joint target. q_des1 = 0.4 q_des2 = 0.7 q_des3 = 0.8 # Slack limits. e1_max = 1000 e2_max = 1000 e3_max = 1000 # Velocity limits.(+-) v0_max = 0.05 v1_max = 0.1 # Acceleration limits.(+-) a0_max = 0.05 * 0.5 a1_max = 0.1 * 0.5 # Others precision = 1e-3 p = 10 q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1 error1 = p * (q_des1 - q_eef) error2 = p * (q_des2 - q_eef) error3 = p * (q_des3 - q_eef) vel_init = 0 nWSR = np.array([100]) # Dynamic goal weights sum_error = abs(error1) + abs(error2) + abs(error3) min_error = min(abs(error1), abs(error2), abs(error3)) max_error = max(abs(error1), abs(error2), abs(error3)) w3 = (1 - (abs(error1) - min_error) / (max_error - min_error))**3 # goal 1 weight w4 = (1 - (abs(error2) - min_error) / (max_error - min_error))**3 # goal 2 weight w5 = (1 - (abs(error3) - min_error) / (max_error - min_error))**3 # goal 3 weight # Acceleration a0_const = (v0_max - a0_max) / v0_max a1_const = (v1_max - a1_max) / v1_max example = SQProblem(5, 7) H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, 0.0, w4, 0.0, 0.0, 0.0, 0.0, 0.0, w5 ]).reshape((5, 5)) A = np.array([ 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 ]).reshape((7, 5)) g = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) lb = np.array([-v0_max, -v1_max, -e1_max, -e2_max, -e3_max]) ub = np.array([v0_max, v1_max, e1_max, e2_max, e3_max]) lbA = np.array([ error1, error2, error3, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max ]) ubA = np.array( [error1, error2, error3, (q0_max - q0), (q1_max - q0), a0_max, a1_max]) # Setting up QProblem object options = Options() options.setToReliable() options.printLevel = PrintLevel.LOW example.setOptions(options) Opt = np.zeros(5) print( "Init pos = %g, goal_1 = %g, goal_2 = %g, error_1 = %g, error_2 = %g, q0 =%g, q1 = %g\n" % (q_eef, q_des1, q_des2, error1, error2, q0, q1)) print 'A = {}\n'.format(A) i = 0 # Stopping conditions limit1 = abs(error1) limit2 = abs(error2) limit3 = abs(error3) lim = min([limit1, limit2, limit3]) diff1 = abs(error1) diff2 = abs(error2) diff3 = abs(error3) diff = min([diff1, diff2, diff3]) # Plotting t = np.array(i) pos_eef = np.array(q_eef) pos_0 = np.array(q0) pos_1 = np.array(q1) vel_0 = np.array(vel_init) vel_1 = np.array(vel_init) vel_eef = np.array(vel_init) p_error = np.array(error1) go_1 = np.array(q_des1) go_2 = np.array(q_des2) go_3 = np.array(q_des3) return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Init of QP-Problem returned without success! ERROR MESSAGE: " return -1 while not rospy.is_shutdown(): while (diff > 0.0009) and lim > precision and i < 400: # Solve QP. i += 1 nWSR = np.array([100]) lbA[0] = error1 lbA[1] = error2 lbA[2] = error3 lbA[3] = -q0_max - q0 lbA[4] = -q1_max - q1 lbA[5] = a0_const * Opt[0] - a0_max lbA[6] = a1_const * Opt[1] - a1_max ubA[0] = error1 ubA[1] = error2 ubA[2] = error3 ubA[3] = q0_max - q0 ubA[4] = q1_max - q1 ubA[5] = a0_const * Opt[0] + a0_max ubA[6] = a1_const * Opt[1] + a1_max return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: " return -1 old_error1 = error1 old_error2 = error2 old_error3 = error3 example.getPrimalSolution(Opt) # Update limits q0 += Opt[0] / 100 q1 += Opt[1] / 100 q_eef = l1 + l2 + l3 + q0 + q1 error1 = p * (q_des1 - q_eef) error2 = p * (q_des2 - q_eef) error3 = p * (q_des3 - q_eef) # Update weights min_error = min(abs(error1), abs(error2), abs(error3)) max_error = max(abs(error1), abs(error2), abs(error3)) w3 = (1 - (abs(error1) - min_error) / (max_error - min_error))**3 # goal 1 w4 = (1 - (abs(error2) - min_error) / (max_error - min_error))**3 # goal 2 w5 = (1 - (abs(error3) - min_error) / (max_error - min_error))**3 # goal 3 H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, 0.0, w4, 0.0, 0.0, 0.0, 0.0, 0.0, w5 ]).reshape((5, 5)) # Stopping conditions limit1 = abs(error1) limit2 = abs(error2) limit3 = abs(error3) lim = min([limit1, limit2, limit3]) diff1 = abs(error1 - old_error1) diff2 = abs(error2 - old_error2) diff3 = abs(error3 - old_error3) diff = min([diff1, diff2, diff3]) # print 'weights= [ %g, %g, %g ]'%(w3, w4, w5) # print 'vel = [ %g, %g ], error = [ %g, %g, %g ] \n'% (Opt[0], Opt[1], error1, error2, error3) # Plotting arrays pos_eef = np.hstack((pos_eef, q_eef)) pos_0 = np.hstack((pos_0, q0)) pos_1 = np.hstack((pos_1, q1)) vel_0 = np.hstack((vel_0, Opt[0])) vel_1 = np.hstack((vel_1, Opt[1])) vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1])) p_error = np.hstack((p_error, error1)) go_1 = np.hstack((go_1, q_des1)) go_2 = np.hstack((go_2, q_des2)) go_3 = np.hstack((go_3, q_des3)) t = np.hstack((t, i)) # Plot t_eef = go.Scatter(y=pos_eef, x=t, marker=dict(size=4, ), mode='lines', name='pos_eef', line=dict(dash='dash')) t_p0 = go.Scatter(y=pos_0, x=t, marker=dict(size=4, ), mode='lines', name='pos_0', line=dict(dash='dash')) t_p1 = go.Scatter(y=pos_1, x=t, marker=dict(size=4, ), mode='lines', name='pos_1', line=dict(dash='dash')) t_v0 = go.Scatter(y=vel_0, x=t, marker=dict(size=4, ), mode='lines', name='vel_0') t_v1 = go.Scatter(y=vel_1, x=t, marker=dict(size=4, ), mode='lines', name='vel_1') t_er = go.Scatter(y=p_error, x=t, marker=dict(size=4, ), mode='lines', name='error') t_veef = go.Scatter(y=vel_eef, x=t, marker=dict(size=4, ), mode='lines', name='vel_eef') t_g1 = go.Scatter(y=go_1, x=t, marker=dict(size=4, ), mode='lines', name='goal_1', line=dict(dash='dot')) t_g2 = go.Scatter(y=go_2, x=t, marker=dict(size=4, ), mode='lines', name='goal_2', line=dict(dash='dot', width=4)) t_g3 = go.Scatter(y=go_3, x=t, marker=dict(size=4, ), mode='lines', name='goal_3', line=dict(dash='dot', width=6)) data = [t_eef, t_veef, t_g1, t_g2, t_g3] layout = dict( title= "Initial position eef = %g. Goals: goal_1 = %g, goal_2 = %g, goal_3 = %g\n" % (q_eef_init, q_des1, q_des2, q_des3), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=2, ), yaxis=dict(title='Position / Velocity'), font=dict(size=18), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_3_weights_EEF.html', image='png', image_filename='dyn_3_eef_2') data = [t_p0, t_v0] layout = dict( title="Initial position q0 =%g.\n" % (q0_init), xaxis=dict(title='Iterations', autotick=False, dtick=25, gridwidth=2), yaxis=dict( title='Position / Velocity', range=[-0.11, .045], ), font=dict(size=18), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_3_weights_q0.html') data = [t_p1, t_v1] layout = dict( title="Initial position q1 = %g.\n" % (q1_init), xaxis=dict(title='Iterations', autotick=False, dtick=25, gridwidth=2), yaxis=dict( title='Position / Velocity', range=[-0.11, .045], ), font=dict(size=18), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_3_weights__q1.html') print "\n i = %g \n" % i return 0
def test_qp_setup_with_toy_example_hack_from_qpoases_manual(self): gen = ClassicGenerator() options = Options() options.printLevel = PrintLevel.LOW # define matrices from qpoases manual H = numpy.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2)) A = numpy.array([1.0, 1.0]).reshape((1, 2)) g = numpy.array([1.5, 1.0]) lb = numpy.array([0.5, -2.0]) ub = numpy.array([5.0, 2.0]) lbA = numpy.array([-1.0]) ubA = numpy.array([2.0]) x = numpy.array([0.5, -1.5]) f = -6.25e-02 # hack pattern generator gen.ori_nv = 2 gen.ori_nc = 1 gen.ori_dofs = numpy.zeros((2, )) gen.ori_qp = SQProblem(gen.ori_nv, gen.ori_nc) gen.ori_qp.setOptions(options) gen.ori_H = H gen.ori_A = A gen.ori_g = g gen.ori_lb = lb gen.ori_ub = ub gen.ori_lbA = lbA gen.ori_ubA = ubA gen.pos_nv = 2 gen.pos_nc = 1 gen.pos_dofs = numpy.zeros((2, )) gen.pos_qp = SQProblem(gen.pos_nv, gen.pos_nc) gen.pos_qp.setOptions(options) gen.pos_H = H gen.pos_A = A gen.pos_g = g gen.pos_lb = lb gen.pos_ub = ub gen.pos_lbA = lbA gen.pos_ubA = ubA # test first qp solution gen._solve_qp() # get solution # NOTE post_process put entries into array that dont match anymore gen.pos_qp.getPrimalSolution(gen.pos_dofs) gen.ori_qp.getPrimalSolution(gen.ori_dofs) assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL) assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL) assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL) assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL) # define matrices for warmstart H_new = numpy.array([1.0, 0.5, 0.5, 0.5]).reshape((2, 2)) A_new = numpy.array([1.0, 5.0]).reshape((1, 2)) g_new = numpy.array([1.0, 1.5]) lb_new = numpy.array([0.0, -1.0]) ub_new = numpy.array([5.0, -0.5]) lbA_new = numpy.array([-2.0]) ubA_new = numpy.array([1.0]) x = numpy.array([0.5, -0.5]) f = -1.875e-01 # hack pattern generator gen.ori_H = H_new gen.ori_A = A_new gen.ori_g = g_new gen.ori_lb = lb_new gen.ori_ub = ub_new gen.ori_lbA = lbA_new gen.pos_H = H_new gen.pos_A = A_new gen.pos_g = g_new gen.pos_lb = lb_new gen.pos_ub = ub_new gen.pos_lbA = lbA_new # test qp warmstart gen._solve_qp() # get solution # NOTE post_process put entries into array that dont match anymore gen.pos_qp.getPrimalSolution(gen.pos_dofs) gen.ori_qp.getPrimalSolution(gen.ori_dofs) assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL) assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL) assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL) assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
def __init__(self, N=16, T=0.1, T_step=0.8, fsm_state='D', fsm_sl=1): """ Initialize pattern generator matrices through base class and allocate two QPs one for optimzation of orientation and one for position of CoM and feet. """ super(ClassicGenerator, self).__init__(N, T, T_step, fsm_state, fsm_sl) # TODO for speed up one can define members of BaseGenerator as # direct views of QP data structures according to walking report # Maybe do that later! # The pattern generator has to solve the following kind of # problem in each iteration # min_x 1/2 * x^T * H(w0) * x + x^T g(w0) # s.t. lbA(w0) <= A(w0) * x <= ubA(w0) # lb(w0) <= x <= ub(wo) # Because of varying H and A, we have to use the # SQPProblem class, which supports this kind of QPs # rename for convenience N = self.N nf = self.nf # define some qpOASES specific things self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit self.nwsr = 100 # number of working set recalculations self.options = Options() self.options.setToMPC() self.options.printLevel = PrintLevel.LOW # FOR ORIENTATIONS # define dimensions self.ori_nv = 2 * self.N self.ori_nc = (self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq) # setup problem self.ori_dofs = numpy.zeros(self.ori_nv) self.ori_qp = SQProblem(self.ori_nv, self.ori_nc) self.ori_qp.setOptions(self.options) # load NMPC options self.ori_H = numpy.zeros((self.ori_nv, self.ori_nv)) self.ori_A = numpy.zeros((self.ori_nc, self.ori_nv)) self.ori_g = numpy.zeros((self.ori_nv, )) self.ori_lb = -numpy.ones((self.ori_nv, )) * 1e+08 self.ori_ub = numpy.ones((self.ori_nv, )) * 1e+08 self.ori_lbA = -numpy.ones((self.ori_nc, )) * 1e+08 self.ori_ubA = numpy.ones((self.ori_nc, )) * 1e+08 self._ori_qp_is_initialized = False # save computation time and working set recalculations self.ori_qp_nwsr = 0.0 self.ori_qp_cputime = 0.0 # FOR POSITIONS # define dimensions self.pos_nv = 2 * (self.N + self.nf) self.pos_nc = (self.nc_cop + self.nc_foot_position + self.nc_fchange_eq) # setup problem self.pos_dofs = numpy.zeros(self.pos_nv) self.pos_qp = SQProblem(self.pos_nv, self.pos_nc) self.pos_qp.setOptions(self.options) self.pos_H = numpy.zeros((self.pos_nv, self.pos_nv)) self.pos_A = numpy.zeros((self.pos_nc, self.pos_nv)) self.pos_g = numpy.zeros((self.pos_nv, )) self.pos_lb = -numpy.ones((self.pos_nv, )) * 1e+08 self.pos_ub = numpy.ones((self.pos_nv, )) * 1e+08 self.pos_lbA = -numpy.ones((self.pos_nc, )) * 1e+08 self.pos_ubA = numpy.ones((self.pos_nc, )) * 1e+08 self._pos_qp_is_initialized = False # save computation time and working set recalculations self.pos_qp_nwsr = 0.0 self.pos_qp_cputime = 0.0 # dummy matrices self._ori_Q = numpy.zeros((2 * self.N, 2 * self.N)) self._ori_p = numpy.zeros((2 * self.N, )) self._pos_Q = numpy.zeros((self.N + self.nf, self.N + self.nf)) self._pos_p = numpy.zeros((self.N + self.nf, )) # add additional keys that should be saved self._data_keys.append('ori_qp_nwsr') self._data_keys.append('ori_qp_cputime') self._data_keys.append('pos_qp_nwsr') self._data_keys.append('pos_qp_cputime') # reinitialize plot data structure self.data = PlotData(self)
def main(): rospy.init_node('controller_2dof', anonymous=True) rospy.Rate(10) # Setup data of QP. # Joint Weights. w1 = 1e-3 w2 = 1e-3 # Joint limits. q0_max = 0.05 q1_max = 0.15 # Links l1 = 0.1 l2 = 0.2 l3 = 0.3 # Initial joint values. q0_init = q0 = 0.01 q1_init = q1 = -0.02 # Joint target. q_des1 = 0.45 q_des2 = 0.78 # Slack limits. e1_max = 1000 e2_max = 1000 # Velocity limits.(+-) v0_max = 0.05 v1_max = 0.1 # Acceleration limits.(+-) a0_max = 0.05 * 0.5 a1_max = 0.1 * 0.5 # Others precision = 1e-2 p = 10 q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1 error1 = p * (q_des1 - q_eef) error2 = p * (q_des2 - q_eef) vel_init = 0 nWSR = np.array([100]) # Dynamic goal weights w3 = (error2 / (error1 + error2))**2 # goal 1 weight w4 = (error1 / (error1 + error2))**2 # goal 2 weight # Acceleration a0_const = (v0_max - a0_max) / v0_max a1_const = (v1_max - a1_max) / v1_max example = SQProblem(4, 6) H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, w4 ]).reshape((4, 4)) A = np.array([ 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 ]).reshape((6, 4)) g = np.array([0.0, 0.0, 0.0, 0.0]) lb = np.array([-v0_max, -v1_max, -e1_max, -e2_max]) ub = np.array([v0_max, v1_max, e1_max, e2_max]) lbA = np.array( [error1, error2, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max]) ubA = np.array( [error1, error2, (q0_max - q0), (q1_max - q0), a0_max, a1_max]) # Setting up QProblem object options = Options() options.setToReliable() options.printLevel = PrintLevel.LOW example.setOptions(options) Opt = np.zeros(4) print( "Init pos = %g, goal_1 = %g, goal_2 = %g, error_1 = %g, error_2 = %g, q0 =%g, q1 = %g\n" % (q_eef, q_des1, q_des2, error1, error2, q0, q1)) print A i = 0 # Stopping conditions limit1 = abs(error1) limit2 = abs(error2) lim = min([limit1, limit2]) diff1 = abs(error1) diff2 = abs(error2) diff = min([diff1, diff2]) # Plotting t = np.array(i) pos_eef = np.array(q_eef) pos_0 = np.array(q0) pos_1 = np.array(q1) vel_0 = np.array(vel_init) vel_1 = np.array(vel_init) vel_eef = np.array(vel_init) p_error = np.array(error1) r_max = np.array(q_des1) r_min = np.array(q_des2) return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Init of QP-Problem returned without success! ERROR MESSAGE: " return -1 while not rospy.is_shutdown(): #while (diff1 > 0.00005 or diff2 > 0.0005) and limit1 > precision and limit2 > precision and i < 400: while diff > 0.00005 and lim > precision and i < 400: # Solve QP. i += 1 nWSR = np.array([100]) lbA[0] = error1 lbA[1] = error2 lbA[2] = -q0_max - q0 lbA[3] = -q1_max - q1 lbA[4] = a0_const * Opt[0] - a0_max lbA[5] = a1_const * Opt[1] - a1_max ubA[0] = error1 ubA[1] = error2 ubA[2] = q0_max - q0 ubA[3] = q1_max - q1 ubA[4] = a0_const * Opt[0] + a0_max ubA[5] = a1_const * Opt[1] + a1_max return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: " return -1 old_error1 = error1 old_error2 = error2 # Update limits example.getPrimalSolution(Opt) q0 += Opt[0] / 100 q1 += Opt[1] / 100 q_eef = l1 + l2 + l3 + q0 + q1 error1 = p * (q_des1 - q_eef) error2 = p * (q_des2 - q_eef) # Update weights w3 = (error2 / (error1 + error2))**2 w4 = (error1 / (error1 + error2))**2 H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, w4 ]).reshape((4, 4)) # Stopping conditions limit1 = abs(error1) limit2 = abs(error2) lim = min([limit1, limit2]) diff1 = abs(error1 - old_error1) diff2 = abs(error2 - old_error2) diff = min([diff1, diff2]) #print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, w3= %g, w4= %g, q0= %g q1= %g \n" % ( #Opt[0], Opt[1], Opt[2], Opt[3], q_eef, w3, w4, q0, q1) print w3, w4 # Plotting arrays pos_eef = np.hstack((pos_eef, q_eef)) pos_0 = np.hstack((pos_0, q0)) pos_1 = np.hstack((pos_1, q1)) vel_0 = np.hstack((vel_0, Opt[0])) vel_1 = np.hstack((vel_1, Opt[1])) vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1])) p_error = np.hstack((p_error, error1)) r_max = np.hstack((r_max, q_des1)) r_min = np.hstack((r_min, q_des2)) t = np.hstack((t, i)) # Plot t_eef = go.Scatter(y=pos_eef, x=t, marker=dict(size=4, ), mode='lines', name='pos_eef', line=dict(dash='dash')) t_p0 = go.Scatter(y=pos_0, x=t, marker=dict(size=4, ), mode='lines', name='pos_0', line=dict(dash='dash')) t_p1 = go.Scatter(y=pos_1, x=t, marker=dict(size=4, ), mode='lines', name='pos_1', line=dict(dash='dash')) t_v0 = go.Scatter(y=vel_0, x=t, marker=dict(size=4, ), mode='lines', name='vel_0') t_v1 = go.Scatter(y=vel_1, x=t, marker=dict(size=4, ), mode='lines', name='vel_1') t_veef = go.Scatter(y=vel_eef, x=t, marker=dict(size=4, ), mode='lines', name='vel_eef') t_er = go.Scatter(y=p_error, x=t, marker=dict(size=4, ), mode='lines', name='error') t_g1 = go.Scatter(y=r_max, x=t, marker=dict(size=4, ), mode='lines', name='goal_1', line=dict(dash='dot')) t_g2 = go.Scatter(y=r_min, x=t, marker=dict(size=4, ), mode='lines', name='goal_2', line=dict(dash='dot', width=4)) data = [t_eef, t_veef, t_g1, t_g2] layout = dict( title="Initial position EEF = %g. Goals: goal_1 = %g, goal_2 = %g" % (q_eef_init, q_des1, q_des2), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, ), font=dict(size=18), yaxis=dict(title='Position / Velocity'), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_weights_eef.html', image='png', image_filename='dynamic_2') data = [t_p0, t_v0] layout = dict(title="Initial position q0 =%g." % (q0_init), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, ), yaxis=dict(title='Position / Velocity', gridwidth=3, range=[-0.15, 0.05]), font=dict(size=18)) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_weights_q0.html', image='png', image_filename='dynamic_3') data = [t_p1, t_v1] layout = dict( title="Initial position q1 = %g." % (q1_init), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, ), yaxis=dict(title='Position / Velocity', gridwidth=3, range=[-0.15, 0.05]), font=dict(size=18), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_weights_q1.html', image='png', image_filename='dynamic_4') print "\n i = %g \n" % i return 0
def test_example2(self): # Example for qpOASES main function using the SQProblem class. # Setup data of first QP. H = np.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2)) A = np.array([ 1.0, 1.0 ]).reshape((2,1)) g = np.array([ 1.5, 1.0 ]) lb = np.array([ 0.5, -2.0 ]) ub = np.array([ 5.0, 2.0 ]) lbA = np.array([ -1.0 ]) ubA = np.array([ 2.0 ]) # Setup data of second QP. H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2)) A_new = np.array([ 1.0, 5.0 ]).reshape((2,1)) g_new = np.array([ 1.0, 1.5 ]) lb_new = np.array([ 0.0, -1.0 ]) ub_new = np.array([ 5.0, -0.5 ]) lbA_new = np.array([ -2.0 ]) ubA_new = np.array([ 1.0 ]) # Setting up SQProblem object and solution analyser. qp = SQProblem(2, 1) options = Options() options.printLevel = PrintLevel.NONE qp.setOptions(options) analyser = SolutionAnalysis() # get c++ solution from std cmd = os.path.join(bin_path, "example2") p = Popen(cmd, shell=True, stdout=PIPE) stdout, stderr = p.communicate() stdout = str(stdout).replace('\\n', '\n') stdout = stdout.replace("'", '') print(stdout) # Solve first QP ... nWSR = 10 qp.init(H, g, A, lb, ub, lbA, ubA, nWSR) # ... and analyse it. maxKKTviolation = np.zeros(1) analyser.getMaxKKTviolation(qp, maxKKTviolation) print("maxKKTviolation: %e\n"%maxKKTviolation) actual = np.asarray(maxKKTviolation) pattern = re.compile(r'maxKKTviolation: (?P<maxKKTviolation>[0-9+-e.]*)') match = pattern.findall(stdout) expected = np.asarray(match[0], dtype=float) assert_almost_equal(actual, expected, decimal=7) # Solve second QP ... nWSR = 10 qp.hotstart(H_new, g_new, A_new, lb_new, ub_new, lbA_new, ubA_new, nWSR) # ... and analyse it. analyser.getMaxKKTviolation(qp, maxKKTviolation) print("maxKKTviolation: %e\n"%maxKKTviolation) actual = np.asarray(maxKKTviolation) expected = np.asarray(match[1], dtype=float) assert_almost_equal(actual, expected, decimal=7) # ------------ VARIANCE-COVARIANCE EVALUATION -------------------- Var = np.zeros(5*5) Primal_Dual_Var = np.zeros(5*5) Var.reshape((5,5))[0,0] = 1. Var.reshape((5,5))[1,1] = 1. # ( 1 0 0 0 0 ) # ( 0 1 0 0 0 ) # Var = ( 0 0 0 0 0 ) # ( 0 0 0 0 0 ) # ( 0 0 0 0 0 ) analyser.getVarianceCovariance(qp, Var, Primal_Dual_Var) print('Primal_Dual_Var=\n', Primal_Dual_Var.reshape((5,5))) actual = Primal_Dual_Var.reshape((5,5)) pattern = re.compile(r'Primal_Dual_VAR = (?P<VAR>.*)', re.DOTALL) print(stdout) match = pattern.search(stdout) expected = match.group('VAR').strip().split("\n") expected = [x.strip().split() for x in expected] print(expected) expected = np.asarray(expected, dtype=float) assert_almost_equal(actual, expected, decimal=7)
def solveLeastSquare(A, b, lb=None, ub=None, A_in=None, lb_in=None, ub_in=None, maxIterations=None, maxComputationTime=60.0, regularization=1e-8): n = A.shape[1] m_in = 0 A = np.asarray(A) b = np.asarray(b).squeeze() if (A_in is not None): m_in = A_in.shape[0] if (lb_in is None): lb_in = np.array(m_in * [-1e99]) else: lb_in = np.asarray(lb_in).squeeze() if (ub_in is None): ub_in = np.array(m_in * [1e99]) else: ub_in = np.asarray(ub_in).squeeze() if (lb is None): lb = np.array(n * [-1e99]) else: lb = np.asarray(lb).squeeze() if (ub is None): ub = np.array(n * [1e99]) else: ub = np.asarray(ub).squeeze() # 0.5||Ax-b||^2 = 0.5(x'A'Ax - 2b'Ax + b'b) = 0.5x'A'Ax - b'Ax +0.5b'b Hess = np.dot(A.T, A) + regularization * np.identity(n) grad = -np.dot(A.T, b) if (maxIterations is None): maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n]) else: maxActiveSetIter = np.array([maxIterations]) maxComputationTime = np.array([maxComputationTime]) options = Options() options.printLevel = PrintLevel.NONE #NONE, LOW, MEDIUM options.enableRegularisation = False if (m_in == 0): qpOasesSolver = QProblemB(n) #, HessianType.SEMIDEF); qpOasesSolver.setOptions(options) # beware that the Hessian matrix may be modified by this function imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter, maxComputationTime) else: qpOasesSolver = SQProblem(n, m_in) #, HessianType.SEMIDEF); qpOasesSolver.setOptions(options) imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in, maxActiveSetIter, maxComputationTime) x = np.empty(n) qpOasesSolver.getPrimalSolution(x) #print "QP cost:", 0.5*(np.linalg.norm(np.dot(A, x)-b)**2); return (imode, np.asmatrix(x).T)
def __init__(self, N=16, T=0.1, T_step=0.8, fsm_state='D', fsm_sl=1): """ Initialize pattern generator matrices through base class and allocate two QPs one for optimzation of orientation and one for position of CoM and feet. """ super(NMPCGenerator, self).__init__(N, T, T_step, fsm_state, fsm_sl) # The pattern generator has to solve the following kind of # problem in each iteration # min_x 1/2 * x.T * H(w0) * x + x.T g(w0) # s.t. lbA(w0) <= A(w0) * x <= ubA(w0) # lb(w0) <= x <= ub(wo) # Because of varying H and A, we have to use the # SQPProblem class, which supports this kind of QPs # rename for convenience N = self.N nf = self.nf # define some qpOASES specific things self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit self.nwsr = 100 # # of working set recalculations self.options = Options() self.options.setToMPC() #self.options.printLevel = PrintLevel.LOW # define variable dimensions # variables of: position + orientation self.nv = 2 * (self.N + self.nf) + 2 * N # define constraint dimensions self.nc_pos = (self.nc_cop + self.nc_foot_position + self.nc_fchange_eq) self.nc_ori = (self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq) self.nc = ( # position self.nc_pos # orientation + self.nc_ori) # setup problem self.dofs = numpy.zeros(self.nv) self.qp = SQProblem(self.nv, self.nc) # load NMPC options self.qp.setOptions(self.options) self.qp_H = numpy.eye(self.nv, self.nv) self.qp_A = numpy.zeros((self.nc, self.nv)) self.qp_g = numpy.zeros((self.nv, )) self.qp_lb = -numpy.ones((self.nv, )) * 1e+08 self.qp_ub = numpy.ones((self.nv, )) * 1e+08 self.qp_lbA = -numpy.ones((self.nc, )) * 1e+08 self.qp_ubA = numpy.ones((self.nc, )) * 1e+08 self._qp_is_initialized = False # save computation time and working set recalculations self.qp_nwsr = 0.0 self.qp_cputime = 0.0 # setup analyzer for solution analysis analyser = SolutionAnalysis() # helper matrices for common expressions self.Hx = numpy.zeros((1, 2 * (N + nf)), dtype=float) self.Q_k_x = numpy.zeros((N + nf, N + nf), dtype=float) self.p_k_x = numpy.zeros((N + nf, ), dtype=float) self.p_k_y = numpy.zeros((N + nf, ), dtype=float) self.Hq = numpy.zeros((1, 2 * N), dtype=float) self.Q_k_qR = numpy.zeros((N, N), dtype=float) self.Q_k_qL = numpy.zeros((N, N), dtype=float) self.p_k_qR = numpy.zeros((N), dtype=float) self.p_k_qL = numpy.zeros((N), dtype=float) self.A_pos_x = numpy.zeros((self.nc_pos, 2 * (N + nf)), dtype=float) self.A_pos_q = numpy.zeros((self.nc_pos, 2 * N), dtype=float) self.ubA_pos = numpy.zeros((self.nc_pos, ), dtype=float) self.lbA_pos = numpy.zeros((self.nc_pos, ), dtype=float) self.A_ori = numpy.zeros((self.nc_ori, 2 * N), dtype=float) self.ubA_ori = numpy.zeros((self.nc_ori, ), dtype=float) self.lbA_ori = numpy.zeros((self.nc_ori, ), dtype=float) self.derv_Acop_map = numpy.zeros((self.nc_cop, self.N), dtype=float) self.derv_Afoot_map = numpy.zeros((self.nc_foot_position, self.N), dtype=float) self._update_foot_selection_matrix() # add additional keys that should be saved self._data_keys.append('qp_nwsr') self._data_keys.append('qp_cputime') # reinitialize plot data structure self.data = PlotData(self)
def test_assembly_of_dofs(self): # define initial values comx = [0.00949035, 0.0, 0.0] comy = [0.095, 0.0, 0.0] comz = 0.814 footx = 0.00949035 footy = 0.095 footq = 0.0 # pattern generator preparation gen = NMPCGenerator() # set reference velocities to zero gen.set_velocity_reference([0.2, 0.0, -0.2]) gen.set_security_margin(0.04, 0.04) # set initial values gen.set_initial_values(comx, comy, comz, footx, footy, footq, foot='left') # hack generator gen.nc = 0 # setup problem gen.dofs = numpy.zeros(gen.nv) gen.qp = SQProblem(gen.nv, gen.nc) # load NMPC options #gen.qp.setOptions(gen.options) gen.H = numpy.eye(gen.nv, gen.nv) gen.A = numpy.zeros((gen.nc, gen.nv)) gen.g = numpy.zeros((gen.nv, )) gen.lb = -numpy.ones((gen.nv, )) * 1e+08 gen.ub = numpy.ones((gen.nv, )) * 1e+08 gen.lbA = -numpy.ones((gen.nc, )) * 1e+08 gen.ubA = numpy.ones((gen.nc, )) * 1e+08 gen._qp_is_initialized = False # renaming for convenience N = gen.N nf = gen.nf # define specified input gen.dddC_k_x[...] = 1.0 gen.F_k_x[...] = 2.0 gen.dddC_k_y[...] = 3.0 gen.F_k_y[...] = 4.0 gen.dddF_k_qR[...] = 5.0 gen.dddF_k_qL[...] = 6.0 dddC_k_x = gen.dddC_k_x.copy() F_k_x = gen.F_k_x.copy() dddC_k_y = gen.dddC_k_y.copy() F_k_y = gen.F_k_y.copy() dddF_k_qR = gen.dddF_k_qR.copy() dddF_k_qL = gen.dddF_k_qL.copy() # do an assembly of the problem gen._preprocess_solution() # check if dofs are assembled correctly assert_allclose(gen.dofs[0:0 + N], gen.dddC_k_x) assert_allclose(gen.dofs[0 + N:0 + N + nf], gen.F_k_x) a = N + nf assert_allclose(gen.dofs[a:a + N], gen.dddC_k_y) assert_allclose(gen.dofs[a + N:a + N + nf], gen.F_k_y) a = 2 * (N + nf) assert_allclose(gen.dofs[a:a + N], gen.dddF_k_qR) assert_allclose(gen.dofs[-N:], gen.dddF_k_qL) # reset initilization gen.H[...] = numpy.eye(gen.nv, gen.nv) gen.A[...] = numpy.zeros((gen.nc, gen.nv)) gen.g[...] = numpy.zeros((gen.nv, )) gen.lb[...] = -numpy.ones((gen.nv, )) * 1e+08 gen.ub[...] = numpy.ones((gen.nv, )) * 1e+08 gen.lbA[...] = -numpy.ones((gen.nc, )) * 1e+08 gen.ubA[...] = numpy.ones((gen.nc, )) * 1e+08 # calculate solution which shall be equal to zero #gen._solve_qp() #assert_allclose(gen.dofs, 0.0, atol=ATOL, rtol=RTOL) # hack solution gen.dofs[0:0 + N] = 1.0 gen.dofs[0 + N:0 + N + nf] = 2.0 a = N + nf gen.dofs[a:a + N] = 3.0 gen.dofs[a + N:a + N + nf] = 4.0 a = 2 * (N + nf) gen.dofs[a:a + N] = 5.0 gen.dofs[-N:] = 6.0 # adjust reference values dddC_k_x[...] = 2.0 F_k_x[...] = 4.0 dddC_k_y[...] = 6.0 F_k_y[...] = 8.0 dddF_k_qR[...] = 10.0 dddF_k_qL[...] = 12.0 # add increment to gen._postprocess_solution() # check if dofs are assembled correctly assert_allclose(gen.dddC_k_x, dddC_k_x) assert_allclose(gen.F_k_x, F_k_x) a = N + nf assert_allclose(gen.dddC_k_y, dddC_k_y) assert_allclose(gen.F_k_y, F_k_y) a = 2 * (N + nf) assert_allclose(gen.dddF_k_qR, dddF_k_qR) assert_allclose(gen.dddF_k_qL, dddF_k_qL)
def qpoases_calculation(self, error_posit): # Variable initialization self._feedback.sim_trajectory = [self.current_state] error_orient = np.array([0.0, 0.0, 0.0]) eef_pose_array = PoseArray() reached_orientation = False self.trajectory_length = 0.0 self.old_dist = 0.0 # Setting up QProblem object vel_calculation = SQProblem(self.problem_size[1], self.problem_size[0]) options = Options() options.setToDefault() options.printLevel = PrintLevel.LOW vel_calculation.setOptions(options) Opt = np.zeros(self.problem_size[1]) i = 0 # Config iai_naive_kinematics_sim, send commands clock = ProjectionClock() velocity_msg = JointState() velocity_msg.name = self.all_joint_names velocity_msg.header.stamp = rospy.get_rostime() velocity_array = [0.0 for x in range(len(self.all_joint_names))] effort_array = [0.0 for x in range(len(self.all_joint_names))] velocity_msg.velocity = velocity_array velocity_msg.effort = effort_array velocity_msg.position = effort_array clock.now = rospy.get_rostime() clock.period.nsecs = 1000000 self.pub_clock.publish(clock) # Plot for debugging '''t = np.array([i]) base_weight = np.array(self.jweights[0, 0]) arm_weight = np.array(self.jweights[4, 4]) triang_weight = np.array(self.jweights[3, 3]) low, pos, high, vel_p, error = [], [], [], [], [] error = np.array([0]) for x in range(8+3): low.append(np.array([self.joint_limits_lower[x]])) pos.append(np.array(self.joint_values[x])) vel_p.append(np.array(Opt[x])) high.append(np.array([self.joint_limits_upper[x]]))#''' tic = rospy.get_rostime() while not self.reach_position or not reached_orientation or not self.reach_pregrasp: i += 1 # Check if client cancelled goal if not self.active_goal_flag: self.success = False break if i > 500: self.abort_goal('time_out') break # Solve QP, redefine limit vectors of A. eef_pose_msg = Pose() nWSR = np.array([100]) [ac_lim_lower, ac_lim_upper] = self.acceleration_limits(Opt) self.joint_dist_lower_lim = self.joint_limits_lower - self.joint_values self.joint_dist_upper_lim = self.joint_limits_upper - self.joint_values self.lbA = np.hstack((error_posit, error_orient, self.joint_dist_lower_lim, ac_lim_lower)) self.ubA = np.hstack((error_posit, error_orient, self.joint_dist_upper_lim, ac_lim_upper)) # Recalculate H matrix self.calculate_weigths(error_posit) vel_calculation = SQProblem(self.problem_size[1], self.problem_size[0]) vel_calculation.setOptions(options) return_value = vel_calculation.init(self.H, self.g, self.A, self.lb, self.ub, self.lbA, self.ubA, nWSR) vel_calculation.getPrimalSolution(Opt) if return_value != returnValue.SUCCESSFUL_RETURN: rospy.logerr("QP-Problem returned without success! ") print 'joint limits: ', self.joint_limits_lower print 'joint values: ', self.joint_values print 'position error:', error_posit / self.prop_gain print 'eef ori: [%.3f %.3f %.3f] ' % ( self.eef_pose.p[0], self.eef_pose.p[1], self.eef_pose.p[2]) print 'goal ori : [%.3f %.3f %.3f] ' % ( self.goal_orient_euler[0], self.goal_orient_euler[0], self.goal_orient_euler[0], ) self.abort_goal('infeasible') break for x, vel in enumerate(Opt[4:self.nJoints]): velocity_msg.velocity[self.start_arm[x]] = vel velocity_msg.velocity[self.triang_list_pos] = Opt[3] velocity_msg.velocity[0] = Opt[0] velocity_msg.velocity[1] = Opt[1] # Recalculate Error in EEF position error_posit, limit_p = self.calc_posit_error(error_posit) # Recalculate Error in EEF orientation eef_orient = qo.rotation_to_quaternion(self.eef_pose.M) error_orient, reached_orientation = self.calc_orient_error( eef_orient, self.goal_quat, 0.35, limit_p) # Get trajectory length self.trajectory_length = self.get_trajectory_length( self.eef_pose.p, self.trajectory_length) # Print velocity for iai_naive_kinematics_sim velocity_msg.header.stamp = rospy.get_rostime() self.pub_velocity.publish(velocity_msg) clock.now = rospy.get_rostime() self.pub_clock.publish(clock) # Action feedback self.action_status.status = 1 self._feedback.sim_status = self.action_status.text = 'Calculating trajectory' self._feedback.sim_trajectory.append(self.current_state) self._feedback.trajectory_length = self.trajectory_length self.action_server.publish_feedback(self.action_status, self._feedback) self.action_server.publish_status() self.success = True # Store EEF pose for plotting eef_pose_msg.position.x = self.eef_pose.p[0] eef_pose_msg.position.y = self.eef_pose.p[1] eef_pose_msg.position.z = self.eef_pose.p[2] eef_pose_msg.orientation.x = eef_orient[0] eef_pose_msg.orientation.y = eef_orient[1] eef_pose_msg.orientation.z = eef_orient[2] eef_pose_msg.orientation.w = eef_orient[3] eef_pose_array.poses.append(eef_pose_msg) # Plot for debuging '''for x in range(8+3): low[x] = np.hstack((low[x], self.joint_limits_lower[x])) pos[x] = np.hstack((pos[x], self.joint_values[x])) vel_p[x] = np.hstack((vel_p[x], Opt[x])) high[x] = np.hstack((high[x], self.joint_limits_upper[x])) e = error_posit / self.prop_gain e_p = sqrt(pow(e[0], 2) + pow(e[1], 2) + pow(e[2], 2)) error = np.hstack((error, e_p)) base_weight = np.hstack((base_weight, self.jweights[0, 0])) arm_weight = np.hstack((arm_weight, self.jweights[4, 4])) triang_weight = np.hstack((triang_weight, self.jweights[3, 3])) t = np.hstack((t, i))#''' # print '\n iter: ', i # goal = euler_from_quaternion(self.goal_quat) # print 'joint_vel: ', Opt[:-6] # print 'error pos: ', error_posit / self.prop_gain # print 'eef ori: [%.3f %.3f %.3f] '%(self.eef_pose.p[0],self.eef_pose.p[1], self.eef_pose.p[2]) # print 'ori error: [%.3f %.3f %.3f] '%(error_orient[0], error_orient[1], error_orient[2]) # print 'goal ori : [%.3f %.3f %.3f] '%(goal[0], goal[1], goal[2]) # print 'slack : ', Opt[-6:] self.final_error = sqrt( pow(error_posit[0], 2) + pow(error_posit[1], 2) + pow(error_posit[2], 2)) eef_pose_array.header.stamp = rospy.get_rostime() eef_pose_array.header.frame_id = self.gripper self.pub_plot.publish(eef_pose_array) # Plot '''plt.style.use('ggplot') plt.plot(t, arm_weight, 'g-.', lw=3, label='arm') plt.plot(t, base_weight, 'k--', lw=3, label='base') plt.plot(t, triang_weight, 'm', lw=2, label='torso') plt.xlabel('Iterations') plt.ylabel('Weights') plt.title('Arm, base and torso weights') plt.grid(True) plt.legend() # plt.show() tikz_save('weights.tex', figureheight='5cm', figurewidth='16cm') plt.cla() plt.plot(t, error, 'k', lw=2) plt.xlabel('Iterations') plt.ylabel('Position Error [m]') plt.title('Position Error') plt.grid(True) plt.legend() # plt.show() tikz_save('error.tex', figureheight='4cm', figurewidth='16cm') plt.cla()#''' '''for x in range(8+3): plt.plot(t, low[x], lw=1) plt.plot(t, pos[x], lw=3) plt.plot(t, vel_p[x], lw=3) plt.plot(t, high[x], lw=1) plt.xlabel('Iterations') plt.ylabel('Position / Velocity') plt.grid(True) # plt.show() tikz_save('joint'+str(x)+'.tex', figureheight='5cm', figurewidth='4.5cm') plt.cla()''' '''plt.plot(t, pos[0], 'r--', lw=3, label='x') plt.plot(t, pos[1], 'b', lw=2, label='y') plt.xlabel('Iterations') plt.ylabel('Base Position [m]') plt.title('Trajectory of the base [m]') plt.grid(True) plt.legend() # plt.show() tikz_save('base_pos.tex', figureheight='5cm', figurewidth='16cm') plt.cla() step = 2 plt.plot(t[0:-1:step], pos[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0') plt.plot(t[0:-1:step], pos[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1') plt.plot(t[0:-1:step], pos[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2') plt.plot(t[0:-1:step], pos[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3') plt.plot(t, pos[7], 'k-.', lw=3, label='j4') plt.plot(t, pos[8], '--', lw=3, label='j5') plt.plot(t, pos[9], lw=3, label='j6') plt.xlabel('Iterations') plt.ylabel('Position [rad]') plt.title('Trajectory of the joints') plt.grid(True) plt.legend(loc='upper left') tikz_save('joint_pos.tex', figureheight='8cm', figurewidth='16cm') plt.cla() plt.plot(t, vel_p[0], 'r--', lw=3, label='x') plt.plot(t, vel_p[1], 'b',lw=2, label='y') plt.xlabel('Iterations') plt.ylabel('Base Velocity [m/sec]') plt.title('Velocity of the base') plt.grid(True) plt.legend() tikz_save('base_vel.tex', figureheight='5cm', figurewidth='16cm') plt.cla() plt.plot(t[0:-1:step], vel_p[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0') plt.plot(t[0:-1:step], vel_p[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1') plt.plot(t[0:-1:step], vel_p[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2') plt.plot(t[0:-1:step], vel_p[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3') plt.plot(t, vel_p[7], 'k-.', lw=3, label='j4') plt.plot(t, vel_p[8], '--', lw=3, label='j5') plt.plot(t, vel_p[9], lw=3, label='j6') plt.xlabel('Iterations') plt.ylabel('Arms Joints Velocity [rad/sec]') plt.title("Velocity of the arm's joints") plt.legend(loc='upper left') plt.grid(True) tikz_save('joint_vel.tex', figureheight='8cm', figurewidth='16cm') plt.cla()#''' toc = rospy.get_rostime() print(toc.secs - tic.secs), 'sec Elapsed' return 0
def main(): rospy.init_node('controller_2dof', anonymous=True) rospy.Rate(10) # rospy.sleep(0.2) # Setup data of QP. # Weights. w1 = 1e-3 w2 = 1e-3 w3 = 1 w4 = 1 # joint goal # Joint limits. q0_max = 0.05 q1_max = 0.15 # Links l1 = 0.1 l2 = 0.2 l3 = 0.3 # Initial joint values. q0_init = q0 = 0.02 q1_init = q1 = -0.15 # Joint target. q_des = 0.6 q0_des = -0.025 q1_des = 0.05 q0_goal = True q1_goal = False # Slack limits. e_max = 1000 e0_max = 1000 # Velocity limits.(+-) v0_max = 0.05 v1_max = 0.1 # Acceleration limits.(+-) a0_max = 0.05 * 0.5 a1_max = 0.1 * 0.5 # Others precision = 5e-3 joint_precision = 5e-3 p = 10 pj = 5 q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1 error = p * (q_des - q_eef) vel_init = 0 nWSR = np.array([100]) # Acceleration a0_const = (v0_max - a0_max) / v0_max a1_const = (v1_max - a1_max) / v1_max example = SQProblem(4, 7) H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, w4 ]).reshape((4, 4)) A = np.array([ 1.0, 1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]).reshape((7, 4)) g = np.array([0.0, 0.0, 0.0, 0.0]) lb = np.array([-v0_max, -v1_max, -e_max, -e0_max]) ub = np.array([v0_max, v1_max, e_max, e0_max]) lbA = np.array( [error, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max, 0, 0]) ubA = np.array([error, (q0_max - q0), (q1_max - q0), a0_max, a1_max, 0, 0]) # Setting up QProblem object. if q0_goal is False and q1_goal is False: print("\nNo joint goal specified\n") return -1 elif (q0_goal is True and q1_goal is False) or (q0_goal is False and q1_goal is True): if q0_goal is True: lbA[5] = (q0_des - q0) ubA[5] = (q0_des - q0) # A[0, 0] = 0.0 A[5, 0] = 1.0 A[5, 3] = 1.0 else: lbA[5] = (q1_des - q1) ubA[5] = (q1_des - q1) A[5, 1] = 1.0 A[5, 3] = 1.0 else: A[0, 0] = 0.0 A[0, 1] = 0.0 A[0, 2] = 0.0 A[5, 0] = 1.0 A[5, 2] = 1.0 A[6, 1] = 1.0 A[6, 3] = 1.0 p = 0 error = 0 lbA[0] = 0 ubA[0] = 0 options = Options() options.setToReliable() options.printLevel = PrintLevel.LOW example.setOptions(options) print("Init pos = %g, goal = %g, error = %g, q0 =%g, q1 = %g\n" % (q_eef, q_des, error, q0, q1)) print A i = 0 limit = abs(error) ok = False Opt = np.zeros(4) # Plotting t = np.array(i) pos_eef = np.array(q_eef) pos_0 = np.array(q0) pos_1 = np.array(q1) vel_0 = np.array(vel_init) vel_1 = np.array(vel_init) vel_eef = np.array(vel_init) p_error = np.array(error) eef_goal = np.array(q_des) q0_set = np.array(q0_des) q1_set = np.array(q1_des) return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Init of QP-Problem returned without success! ERROR MESSAGE: " return -1 while not rospy.is_shutdown(): while (limit > precision or not ok) and i < 2000: # Solve QP. i += 1 nWSR = np.array([100]) lbA[0] = error lbA[1] = -q0_max - q0 lbA[2] = -q1_max - q1 lbA[3] = a0_const * Opt[0] - a0_max lbA[4] = a1_const * Opt[1] - a1_max ubA[0] = error ubA[1] = q0_max - q0 ubA[2] = q1_max - q1 ubA[3] = a0_const * Opt[0] + a0_max ubA[4] = a1_const * Opt[1] + a1_max return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: rospy.logerr( "Hotstart of QP-Problem returned without success!") return -1 # Get and print solution of QP. example.getPrimalSolution(Opt) q0 += Opt[0] / 100 q1 += Opt[1] / 100 q_eef = l1 + l2 + l3 + q0 + q1 error = p * (q_des - q_eef) limit = abs(error) # print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, error= %g, q0= %g q1= %g \n" % ( # Opt[0], Opt[1], Opt[2], Opt[3], q_eef, error, q0, q1) # Depending on joint goals if q0_goal is False and q1_goal is False: ok = True error = 0 limit = 0 elif q0_goal is True and q1_goal is False: lbA[5] = pj * (q0_des - q0) ubA[5] = pj * (q0_des - q0) if abs(lbA[5]) < joint_precision: ok = True # print "\n q0_error = %g, i = %g \n" % (lbA[5], i) else: ok = False elif q0_goal is False and q1_goal is True: lbA[5] = pj * (q1_des - q1) ubA[5] = pj * (q1_des - q1) if abs(lbA[5]) < joint_precision: ok = True # print "\n q0_error = %g, i = %g \n" % (lbA[5], i) else: lbA[5] = pj * (q0_des - q0) ubA[5] = pj * (q0_des - q0) lbA[6] = pj * (q1_des - q1) ubA[6] = pj * (q1_des - q1) if abs(lbA[5]) < joint_precision and abs( lbA[6]) < joint_precision: ok = True # print "\n q0_error = %g, q1_error = %g \n" % (lbA[5], lbA[6]) # Plotting arrays pos_eef = np.hstack((pos_eef, q_eef)) pos_0 = np.hstack((pos_0, q0)) pos_1 = np.hstack((pos_1, q1)) vel_0 = np.hstack((vel_0, Opt[0])) vel_1 = np.hstack((vel_1, Opt[1])) vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1])) p_error = np.hstack((p_error, error)) eef_goal = np.hstack((eef_goal, q_des)) q0_set = np.hstack((q0_set, q0_des)) q1_set = np.hstack((q1_set, q1_des)) t = np.hstack((t, i)) # Plot t_eef = go.Scatter(y=pos_eef, x=t, marker=dict(size=4, ), mode='lines', name='pos_eef', line=dict(dash='dash')) t_p0 = go.Scatter(y=pos_0, x=t, marker=dict(size=4, ), mode='lines', name='pos_q0', line=dict(dash='dash')) t_p1 = go.Scatter(y=pos_1, x=t, marker=dict(size=4, ), mode='lines', name='pos_q1', line=dict(dash='dash')) t_v0 = go.Scatter(y=vel_0, x=t, marker=dict(size=4, ), mode='lines', name='vel_q0') t_v1 = go.Scatter(y=vel_1, x=t, marker=dict(size=4, ), mode='lines', name='vel_q1') t_veef = go.Scatter(y=vel_eef, x=t, marker=dict(size=4, ), mode='lines', name='vel_eef') t_er = go.Scatter(y=p_error, x=t, marker=dict(size=4, ), mode='lines', name='error') t_g1 = go.Scatter(y=q0_set, x=t, marker=dict(size=4, ), mode='lines', name='joint0_goal', line=dict(dash='dot')) t_g2 = go.Scatter(y=q1_set, x=t, marker=dict(size=4, ), mode='lines', name='joint1_goal', line=dict(dash='dot')) t_goal = go.Scatter(y=eef_goal, x=t, marker=dict(size=4, ), mode='lines', name='eef_goal', line=dict(dash='dot')) if q0_goal == True: data_q0 = [t_p0, t_v0, t_g1] data_q1 = [t_p1, t_v1] else: data_q0 = [t_p0, t_v0] data_q1 = [t_p1, t_v1, t_g2] layout = dict(title="Initial position q0 =%g.\n" % (q0_init), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, tickcolor='#060', ), yaxis=dict(title='Position / Velocity', gridwidth=3, range=[-0.15, .1]), font=dict(size=18)) fig0 = dict(data=data_q0, layout=layout) plotly.offline.plot(fig0, filename='joint_goals_q0.html', image='png', image_filename='joint_q0') layout = dict(title="Initial position q1 = %g.\n" % (q1_init), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, tickcolor='#060', ), yaxis=dict( title='Position / Velocity', gridwidth=3, ), font=dict(size=18)) fig1 = dict(data=data_q1, layout=layout) plotly.offline.plot(fig1, filename='joint_goals_q1.html', image='png', image_filename='joint_q1') data_eef = [t_eef, t_veef, t_goal] layout_eef = dict(title="Initial position EEF = %g. Goal = %g, \n" % (q_eef_init, q_des), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, ), yaxis=dict(title='Position / Velocity', gridwidth=3), font=dict(size=16)) fig = dict(data=data_eef, layout=layout_eef) plotly.offline.plot(fig, filename='joint_goals_eef.html', image='png', image_filename='joint_eef') print "\n i = ", i return 0 print ok
def main(): rospy.init_node('controller_2dof', anonymous=True) rospy.Rate(10) rospy.sleep(0.2) # Setup data of QP. # Weights. w1 = 1e-3 w2 = 1e-5 w3 = 1 # slack (attractor) # Joint limits. q0_max = 0.05 q1_max = 0.15 # Links l1 = 0.1 l2 = 0.2 l3 = 0.3 # Initial joint values. q0_init = q0 = -0.04 q1_init = q1 = -0.08 # Joint target. q_des = 0.65 # Slack limits. e_max = 1000 # Velocity limits.(+-) v0_max = 0.05 v1_max = 0.1 # Acceleration limits.(+-) a0_max = 0.05 * 0.5 a1_max = 0.1 * 0.5 # Others precision = 1e-3 p = 10 q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1 error = p * (q_des - q_eef) vel_init = 0 nWSR = np.array([100]) # Acceleration a0_const = (v0_max - a0_max) / v0_max a1_const = (v1_max - a1_max) / v1_max example = SQProblem(3, 5) H = np.array([w1, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, w3]).reshape((3, 3)) A = np.array([ 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0 ]).reshape((5, 3)) g = np.array([0.0, 0.0, 0.0, 0.0]) lb = np.array([-v0_max, -v1_max, -e_max]) ub = np.array([v0_max, v1_max, e_max]) lbA = np.array([error, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max]) ubA = np.array([error, (q0_max - q0), (q1_max - q0), a0_max, a1_max]) # Setting up QProblem object options = Options() options.setToReliable() options.printLevel = PrintLevel.LOW example.setOptions(options) print("Init pos = %g, goal = %g, error = %g, q0 =%g, q1 = %g\n" % (q_eef, q_des, error, q0, q1)) print A i = 0 n = 0 limit = abs(error) ok = False Opt = np.zeros(3) # Plotting t = np.array(i) pos_eef = np.array(q_eef) pos_0 = np.array(q0) pos_1 = np.array(q1) vel_0 = np.array(vel_init) vel_1 = np.array(vel_init) vel_eef = np.array(vel_init) p_error = np.array(error) goal = np.array(q_des) lim1 = np.array(q0_max) lim2 = np.array(q1_max) return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Init of QP-Problem returned without success! ERROR MESSAGE: " return -1 while not rospy.is_shutdown(): while limit > precision and i < 400: # Solve QP. i += 1 nWSR = np.array([100]) lbA[0] = error lbA[1] = -q0_max - q0 lbA[2] = -q1_max - q1 lbA[3] = a0_const * Opt[0] - a0_max lbA[4] = a1_const * Opt[1] - a1_max ubA[0] = error ubA[1] = q0_max - q0 ubA[2] = q1_max - q1 ubA[3] = a0_const * Opt[0] + a0_max ubA[4] = a1_const * Opt[1] + a1_max return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: " return -1 # Get and print solution of QP. example.getPrimalSolution(Opt) q0 += Opt[0] / 100 q1 += Opt[1] / 100 q_eef = l1 + l2 + l3 + q0 + q1 error = p * (q_des - q_eef) limit = abs(error) # print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, error= %g, q0= %g q1= %g \n" % ( # Opt[0], Opt[1], Opt[2], Opt[3], q_eef, error, q0, q1) # Plotting arrays pos_eef = np.hstack((pos_eef, q_eef)) pos_0 = np.hstack((pos_0, q0)) pos_1 = np.hstack((pos_1, q1)) vel_0 = np.hstack((vel_0, Opt[0])) vel_1 = np.hstack((vel_1, Opt[1])) vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1])) goal = np.hstack((goal, q_des)) p_error = np.hstack((p_error, error)) t = np.hstack((t, i)) lim1 = np.hstack((lim1, q0_max)) lim2 = np.hstack((lim2, q1_max)) # Plot t_eef = go.Scatter(y=pos_eef, x=t, marker=dict(size=4, ), mode='lines', name='pos_eef', line=dict(dash='dash')) t_p0 = go.Scatter(y=pos_0, x=t, marker=dict(size=4, ), mode='lines', name='pos_q0', line=dict(dash='dash')) t_p1 = go.Scatter(y=pos_1, x=t, marker=dict(size=4, ), mode='lines', name='pos_q1', line=dict(dash='dash')) t_v0 = go.Scatter(y=vel_0, x=t, marker=dict(size=4, ), mode='lines', name='vel_q0') t_v1 = go.Scatter(y=vel_1, x=t, marker=dict(size=4, ), mode='lines', name='vel_q1') t_q0 = go.Scatter(y=lim1, x=t, marker=dict(size=4, ), mode='lines', name='limit_0') t_q1 = go.Scatter(y=lim2, x=t, marker=dict(size=4, ), mode='lines', name='limit_1') t_veef = go.Scatter(y=vel_eef, x=t, marker=dict(size=4, ), mode='lines', name='vel_eef') t_er = go.Scatter(y=p_error, x=t, marker=dict(size=4, ), mode='lines+markers', name='error') t_goal = go.Scatter(y=goal, x=t, marker=dict(size=4, ), mode='lines', name='goal', line=dict(dash='dot')) print "\n i = %g \n" % (i) data_eef = [t_eef, t_veef, t_goal] layout_eef = dict(title="Initial position EEF = %g. Goal = %g, \n" % (q_eef_init, q_des), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=2, ), yaxis=dict(title='Position / Velocity'), font=dict(size=16)) fig = dict(data=data_eef, layout=layout_eef) plotly.offline.plot(fig, filename='html/basic_example_eef.html', image='png', image_filename='basic_eef') data = [t_p0, t_v0] layout = dict(title="Initial position q0 =%g, q1 = %g.\n" % (q0_init, q1_init), xaxis=dict(title='Iterations', autotick=False, dtick=25, gridwidth=2), yaxis=dict( title='Position / Velocity', range=[-0.08, .12], ), font=dict(size=18)) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/basic_example_q0.html', image='png', image_filename='basic_0') data = [t_p1, t_v1] layout = dict(title="Initial position q0 =%g, q1 = %g.\n" % (q0_init, q1_init), xaxis=dict(title='Iterations', autotick=False, dtick=25, gridwidth=2), yaxis=dict( title='Position / Velocity', range=[-0.08, .12], ), font=dict(size=18)) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/basic_example_q1.html', image='png', image_filename='basic_1') return 0