def test_m44_reliable_sparse(self): test_name = 'mm44_reliable_sparse.txt' print(test_name) # QP Options options = Options() options.setToReliable() options.printLevel = PrintLevel.NONE isSparse = True useHotstarts = False # run QP benchmarks results = run_benchmarks(benchmarks, options, isSparse, useHotstarts, self.nWSR, self.cpu_time, self.TOL) # print and write results string = results2str(results) print(string) write_results(test_name, string) assert get_nfail(results) <= 0, 'One ore more tests failed.'
def 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 ComAccLP(object): """ LP solver dedicated to finding the maximum center of mass (CoM) deceleration in a specified direction, subject to the friction cone constraints. This is possible thanks to the simplifying assumption that the CoM acceleration is going to be parallel to its velocity. This allows us to represent the 3d com trajectory by means of a 1d trajectory alpha(t): c(t) = c0 + alpha(t) v v = c0 / ||c0|| dc = dAlpha(t) v ddc(t) = ddAlpha(t) v The operation amounts to solving the following parametric Linear Program: minimize ddAlpha + w sum_i(f_i) subject to A f = a ddAlpha + b alpha + d f >= 0 where: f are the contact forces generator coefficients ddAlpha is the magnitude of the CoM acceleration alpha is the magnitude of the CoM displacement (with respect to its initial position c0) w regularization parameter Given a CoM position (by means of a value of alpha), this class can compute the minimum com acceleration in direction v (i.e. the maximum acceleration in direction -v). Since this is a piecewise-linear function of alpha, it can also compute its derivative with respect to alpha, and the boundaries of the alpha-region in which the derivative remains constant. """ NO_WARM_START = False name = "" # solver name n = 0 # number of variables m_in = 0 # number of constraints (i.e. 6) Hess = [] # Hessian grad = [] # gradient A = None # constraint matrix multiplying the contact force generators b = None # constraint vector multiplying the CoM position parameter alpha d = None # constraint vector mass = 0.0 # robot mass g = None # 3d gravity vector maxIter = 0 # max number of iterations verb = 0 # verbosity level of the solver (0=min, 2=max) iter = 0 # current iteration number computationTime = 0.0 # total computation time qpTime = 0.0 # time taken to solve the QP(s) only initialized = False # true if solver has been initialized qpOasesSolver = [] options = [] # qp oases solver's options epsilon = np.sqrt(np.finfo(float).eps) INEQ_VIOLATION_THR = 1e-4 def __init__(self, name, c0, v, contact_points, contact_normals, mu, g, mass, maxIter=10000, verb=0, regularization=1e-5): ''' Constructor @param c0 Initial CoM position @param v Opposite of the direction in which you want to maximize the CoM acceleration (typically that would be the CoM velocity direction) @param g Gravity vector @param regularization Weight of the force minimization, the higher this value, the sparser the solution ''' self.name = name self.maxIter = maxIter self.verb = verb self.m_in = 6 self.initialized = False self.options = Options() self.options.setToReliable() if (self.verb <= 1): self.options.printLevel = PrintLevel.NONE elif (self.verb == 2): self.options.printLevel = PrintLevel.LOW elif (self.verb == 3): self.options.printLevel = PrintLevel.MEDIUM elif (self.verb > 3): self.options.printLevel = PrintLevel.DEBUG_ITER self.options.enableRegularisation = False self.options.enableEqualities = True # self.qpOasesSolver.printOptions() self.b = np.zeros(6) self.d = np.empty(6) self.c0 = np.empty(3) self.v = np.empty(3) self.constrUB = np.zeros(self.m_in) + 1e100 self.constrLB = np.zeros(self.m_in) - 1e100 self.set_problem_data(c0, v, contact_points, contact_normals, mu, g, mass, regularization) def set_com_state(self, c0, v): assert np.asarray( c0).squeeze().shape[0] == 3, "Com position vector has not size 3" assert np.asarray(v).squeeze( ).shape[0] == 3, "Com acceleration direction vector has not size 3" self.c0 = np.asarray(c0).squeeze() self.v = np.asarray(v).squeeze().copy() if (norm(v) == 0.0): raise ValueError( "[%s] Norm of com acceleration direction v is zero!" % self.name) self.v /= norm(self.v) self.constrMat[:3, -1] = self.mass * self.v self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v) self.b[3:] = self.mass * np.cross(v, self.g) self.d[:3] = self.mass * self.g self.d[3:] = self.mass * np.cross(c0, self.g) def set_contacts(self, contact_points, contact_normals, mu, regularization=1e-5): # compute matrix A, which maps the force generator coefficients into the centroidal wrench (self.A, self.G4) = compute_centroidal_cone_generators(contact_points, contact_normals, mu) # since the size of the problem may have changed we need to recreate the solver and all the problem matrices/vectors self.n = contact_points.shape[0] * 4 + 1 self.qpOasesSolver = SQProblem(self.n, self.m_in) #, HessianType.SEMIDEF); self.qpOasesSolver.setOptions(self.options) self.Hess = INITIAL_HESSIAN_REGULARIZATION * np.identity(self.n) self.grad = np.ones(self.n) * regularization self.grad[-1] = 1.0 self.constrMat = np.zeros((self.m_in, self.n)) self.constrMat[:, :-1] = self.A self.constrMat[:3, -1] = self.mass * self.v self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v) self.lb = np.zeros(self.n) self.lb[-1] = -1e100 self.ub = np.array(self.n * [ 1e100, ]) self.x = np.zeros(self.n) self.y = np.zeros(self.n + self.m_in) self.initialized = False def set_problem_data(self, c0, v, contact_points, contact_normals, mu, g, mass, regularization=1e-5): assert g.shape[0] == 3, "Gravity vector has not size 3" assert mass > 0.0, "Mass is not positive" self.mass = mass self.g = np.asarray(g).squeeze() self.set_contacts(contact_points, contact_normals, mu, regularization) self.set_com_state(c0, v) def compute_max_deceleration_derivative(self): ''' Compute the derivative of the max CoM deceleration (i.e. the solution of the last LP) with respect to the parameter alpha (i.e. the CoM position parameter). Moreover, it also computes the bounds within which this derivative is valid (alpha_min, alpha_max). ''' act_set = np.where(self.y[:self.n - 1] != 0.0)[0] # indexes of active bound constraints n_as = act_set.shape[0] if (n_as > self.n - 6): raise ValueError( "[%s] ERROR Too many active constraints: %d (rather than %d)" % (self.name, n_as, self.n - 6)) if (self.verb > 0 and n_as < self.n - 6): print "[%s] INFO Less active constraints than expected: %d (rather than %d)" % ( self.name, n_as, self.n - 6) self.K = np.zeros((n_as + 6, self.n)) self.k1 = np.zeros(n_as + 6) self.k2 = np.zeros(n_as + 6) self.K[:n_as, :] = np.identity(self.n)[act_set, :] self.K[-6:, :] = self.constrMat self.k1[-6:] = self.b self.k2[-6:] = self.d U, s, VT = np.linalg.svd(self.K) rank = (s > EPS).sum() K_inv_k1 = np.dot(VT[:rank, :].T, (1.0 / s[:rank]) * np.dot(U[:, :rank].T, self.k1)) K_inv_k2 = np.dot(VT[:rank, :].T, (1.0 / s[:rank]) * np.dot(U[:, :rank].T, self.k2)) if (rank < self.n): Z = VT[rank:, :].T P = np.dot( np.dot(Z, np.linalg.inv(np.dot(Z.T, np.dot(self.Hess, Z)))), Z.T) K_inv_k1 -= np.dot(P, np.dot(self.Hess, K_inv_k1)) K_inv_k2 -= np.dot(P, np.dot(self.Hess, K_inv_k2) + self.grad) # Check that the solution you get by solving the KKT is the same found by the solver x_kkt = K_inv_k1 * self.alpha + K_inv_k2 if (norm(self.x - x_kkt) > 10 * EPS): warnings.warn("[%s] ERROR x different from x_kkt. x=" % (self.name) + str(self.x) + "\nx_kkt=" + str(x_kkt) + " " + str(norm(self.x - x_kkt))) # store the derivative of the solution w.r.t. the parameter alpha dx = K_inv_k1[-1] # act_set_mat * alpha >= act_set_vec act_set_mat = K_inv_k1[:-1] act_set_vec = -K_inv_k2[:-1] for i in range(act_set_mat.shape[0]): if (abs(act_set_mat[i]) > EPS): act_set_vec[i] /= abs(act_set_mat[i]) act_set_mat[i] /= abs(act_set_mat[i]) if (act_set_mat * self.alpha < act_set_vec - EPS).any(): raise ValueError( "ERROR: after normalization current alpha violates constraints " + str(act_set_mat * self.alpha - act_set_vec)) ind_pos = np.where(act_set_mat > EPS)[0] if (ind_pos.shape[0] > 0): alpha_min = np.max(act_set_vec[ind_pos]) else: alpha_min = -1e10 # warnings.warn("[%s] alpha_min seems unbounded %.7f"%(self.name, np.max(act_set_mat))); ind_neg = np.where(act_set_mat < -EPS)[0] if (ind_neg.shape[0] > 0): alpha_max = np.min(-act_set_vec[ind_neg]) else: alpha_max = 1e10 # warnings.warn("[%s] alpha_max seems unbounded %.7f"%(self.name, np.min(act_set_mat))); if (alpha_min > alpha_max): raise ValueError("ERROR alpha_min %.3f > alpha_max %.3f" % (alpha_min, alpha_max)) return (dx, alpha_min, alpha_max) def compute_max_deceleration(self, alpha, maxIter=None, maxTime=100.0): start = time.time() self.alpha = alpha if (self.NO_WARM_START): self.qpOasesSolver = SQProblem(self.n, self.m_in) self.qpOasesSolver.setOptions(self.options) self.initialized = False if (maxIter == None): maxIter = self.maxIter maxActiveSetIter = np.array([maxIter]) maxComputationTime = np.array(maxTime) self.constrUB[:6] = np.dot(self.b, alpha) + self.d self.constrLB[:6] = self.constrUB[:6] while (True): if (not self.initialized): self.imode = self.qpOasesSolver.init(self.Hess, self.grad, self.constrMat, self.lb, self.ub, self.constrLB, self.constrUB, maxActiveSetIter, maxComputationTime) else: self.imode = self.qpOasesSolver.hotstart( self.grad, self.lb, self.ub, self.constrLB, self.constrUB, maxActiveSetIter, maxComputationTime) if (self.imode == 0): self.initialized = True if (self.imode == 0 or self.imode == PyReturnValue.INIT_FAILED_INFEASIBILITY or self.imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY or self.Hess[0, 0] >= MAX_HESSIAN_REGULARIZATION): break self.initialized = False self.Hess *= 10.0 maxActiveSetIter = np.array([maxIter]) maxComputationTime = np.array(maxTime) if (self.verb > -1): print "[%s] WARNING %s. Increasing Hessian regularization to %f" % ( self.name, qpOasesSolverMsg(self.imode), self.Hess[0, 0]) self.qpTime = maxComputationTime self.iter = 1 + maxActiveSetIter[0] if (self.imode == 0): self.qpOasesSolver.getPrimalSolution(self.x) self.qpOasesSolver.getDualSolution(self.y) if ((self.x < self.lb - self.INEQ_VIOLATION_THR).any()): self.initialized = False raise ValueError("[%s] ERROR lower bound violated" % (self.name) + str(self.x) + str(self.lb)) if ((self.x > self.ub + self.INEQ_VIOLATION_THR).any()): self.initialized = False raise ValueError("[%s] ERROR upper bound violated" % (self.name) + str(self.x) + str(self.ub)) if ((np.dot(self.constrMat, self.x) > self.constrUB + self.INEQ_VIOLATION_THR).any()): self.initialized = False raise ValueError( "[%s] ERROR constraint upper bound violated " % (self.name) + str(np.min(np.dot(self.constrMat, self.x) - self.constrUB))) if ((np.dot(self.constrMat, self.x) < self.constrLB - self.INEQ_VIOLATION_THR).any()): self.initialized = False raise ValueError( "[%s] ERROR constraint lower bound violated " % (self.name) + str(np.max(np.dot(self.constrMat, self.x) - self.constrLB))) (dx, alpha_min, alpha_max) = self.compute_max_deceleration_derivative() else: self.initialized = False dx = 0.0 alpha_min = 0.0 alpha_max = 0.0 if (self.verb > 0): print "[%s] ERROR Qp oases %s" % (self.name, qpOasesSolverMsg(self.imode)) if (self.qpTime >= maxTime): if (self.verb > 0): print "[%s] Max time reached %f after %d iters" % ( self.name, self.qpTime, self.iter) self.imode = 9 self.computationTime = time.time() - start return (self.imode, self.x[-1], dx, alpha_min, alpha_max) def getContactForces(self): ''' Get the contact forces obtained by solving the last LP ''' cg = 4 nContacts = self.G4.shape[1] / cg f = np.empty((3, nContacts)) for i in range(nContacts): f[:, i] = np.dot(self.G4[:, cg * i:cg * i + cg], self.x[cg * i:cg * i + cg]) return f def reset(self): self.initialized = False
class AbstractSolver(object): """ Abstract solver class to use as base for all solvers. The basic problem has the following structure: minimize 0.5*||D*x - d||^2 subject to Alb <= A*x <= Aub lb <= x <= ub NB: Before it was: subject to G*x + g >= 0 where """ NO_WARM_START = False name = "" # solver name n = 0 # number of variables m_in = -1 # number of inequalities D = [] # quadratic cost matrix d = [] # quadratic cost vector G = [] # inequality matrix g = [] # inequality vector bounds = [] # bounds on the problem variables H = [] # Hessian H = G^T*G dD = [] # Product dD = d^T*D x0 = [] # initial guess solver = '' # type of solver to use accuracy = 0 # accuracy used by the solver for termination maxIter = 0 # max number of iterations verb = 0 # verbosity level of the solver (0=min, 2=max) iter = 0 # current iteration number computationTime = 0.0 # total computation time qpTime = 0.0 # time taken to solve the QP(s) only iterationNumber = 0 # total number of iterations approxProb = 0 # approximated probability of G*x+g>0 initialized = False # true if solver has been initialized nActiveInequalities = 0 # number of active inequality constraints nViolatedInequalities = 0 # number of violated inequalities outerIter = 0 # number of outer (Newton) iterations qpOasesSolver = [] options = [] # qp oases solver's options softInequalityIndexes = [] epsilon = np.sqrt(np.finfo(float).eps) INEQ_VIOLATION_THR = 1e-4 def __init__(self, n, m_in, solver='slsqp', accuracy=1e-6, maxIter=100, verb=0): self.name = "AbstractSolver" self.n = n self.iter = 0 self.solver = solver self.accuracy = accuracy self.maxIter = maxIter self.verb = verb self.qpOasesAnalyser = SolutionAnalysis() self.changeInequalityNumber(m_in) return def setSoftInequalityIndexes(self, indexes): self.softInequalityIndexes = indexes def changeInequalityNumber(self, m_in): # print "[%s] Changing number of inequality constraints from %d to %d" % (self.name, self.m_in, m_in); if (m_in == self.m_in): return self.m_in = m_in self.iter = 0 self.qpOasesSolver = SQProblem(self.n, m_in) #, HessianType.POSDEF SEMIDEF self.options = Options() self.options.setToReliable() if (self.verb <= 0): self.options.printLevel = PrintLevel.NONE elif (self.verb == 1): self.options.printLevel = PrintLevel.LOW elif (self.verb == 2): self.options.printLevel = PrintLevel.MEDIUM elif (self.verb > 2): self.options.printLevel = PrintLevel.DEBUG_ITER print "set high print level" # self.options.enableRegularisation = False; # self.options.enableFlippingBounds = BooleanType.FALSE # self.options.initialStatusBounds = SubjectToStatus.INACTIVE # self.options.setToMPC(); # self.qpOasesSolver.printOptions(); self.qpOasesSolver.setOptions(self.options) self.initialized = False def setProblemData(self, D, d, A, lbA, ubA, lb, ub, x0=None): self.D = D self.d = d.squeeze() if (A.shape[0] == self.m_in and A.shape[1] == self.n): self.A = A self.lbA = lbA.squeeze() self.ubA = ubA.squeeze() else: print "[%s] ERROR. Wrong size of the constraint matrix, %d rather than %d" % ( self.name, A.shape[0], self.m_in) if (lb.shape[0] == self.n and ub.shape[0] == self.n): self.lb = lb.squeeze() self.ub = ub.squeeze() else: print "[%s] ERROR. Wrong size of the bound vectors, %d and %d rather than %d" % ( self.name, lb.shape[0], ub.shape[0], self.n) # self.bounds = self.n*[(-1e10,1e10)]; if (x0 is None): self.x0 = np.zeros(self.n) else: self.x0 = x0.squeeze() self.H = np.dot(self.D.T, self.D) self.dD = np.dot(self.D.T, self.d) def solve(self, D, d, A, lbA, ubA, lb, ub, x0=None, maxIter=None, maxTime=100.0): if (self.NO_WARM_START): self.qpOasesSolver = SQProblem(self.n, self.m_in) self.qpOasesSolver.setOptions(self.options) self.initialized = False if (maxIter is None): maxIter = self.maxIter self.iter = 0 self.qpTime = 0.0 self.removeSoftInequalities = False self.setProblemData(D, d, A, lbA, ubA, lb, ub, x0) start = time.time() x = np.zeros(self.x0.shape) if (self.solver == 'slsqp'): (x, fx, self.iterationNumber, imode, smode) = fmin_slsqp(self.f_cost, self.x0, fprime=self.f_cost_grad, f_ieqcons=self.f_inequalities, fprime_ieqcons=self.f_inequalities_jac, bounds=self.bounds, iprint=self.verb, iter=maxIter, acc=self.accuracy, full_output=1) self.fx = fx x = np.array(x) ''' Exit modes are defined as follows : -1 : Gradient evaluation required (g & a) 0 : Optimization terminated successfully. 1 : Function evaluation required (f & c) 2 : More equality constraints than independent variables 3 : More than 3*n iterations in LSQ subproblem 4 : Inequality constraints incompatible 5 : Singular matrix E in LSQ subproblem 6 : Singular matrix C in LSQ subproblem 7 : Rank-deficient equality constraint subproblem HFTI 8 : Positive directional derivative for linesearch 9 : Iteration limit exceeded ''' if (self.verb > 0 and imode != 0 and imode != 9): #do not print error msg if iteration limit exceeded print "[%s] *** ERROR *** %s" % (self.name, smode) elif (self.solver == 'qpoases'): # ubA = np.array(self.m_in*[1e9]); # lb = np.array([ b[0] for b in self.bounds]); # ub = np.array([ b[1] for b in self.bounds]); # A = self.get_linear_inequality_matrix(); self.iter = 0 #total iters of qpoases # lbA = -self.get_linear_inequality_vector(); Hess = self.f_cost_hess(x) grad = self.f_cost_grad(x) self.fx = self.f_cost(x) maxActiveSetIter = np.array([maxIter - self.iter]) maxComputationTime = np.array(maxTime) if (self.initialized == False): imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb, self.ub, self.lbA, self.ubA, maxActiveSetIter, maxComputationTime) if (imode == 0): self.initialized = True else: imode = self.qpOasesSolver.hotstart(Hess, grad, self.A, self.lb, self.ub, self.lbA, self.ubA, maxActiveSetIter, maxComputationTime) if (imode == PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED): maxActiveSetIter = np.array([maxIter]) maxComputationTime = np.array(maxTime) imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb, self.ub, self.lbA, self.ubA, maxActiveSetIter, maxComputationTime) if (imode == 0): self.initialized = True self.qpTime += maxComputationTime self.iter = 1 + maxActiveSetIter[0] self.iterationNumber = self.iter ''' if the solution found is unfeasible check whether the initial guess is feasible ''' self.qpOasesSolver.getPrimalSolution(x) ineq_marg = self.f_inequalities(x) qpUnfeasible = False if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): qpUnfeasible = True if (x0 is not None): ineq_marg = self.f_inequalities(x0) if (not (ineq_marg < -self.INEQ_VIOLATION_THR).any()): if (self.verb > 0): print "[%s] Solution found is unfeasible but initial guess is feasible" % ( self.name) qpUnfeasible = False x = np.copy(x0) ''' if both the solution found and the initial guess are unfeasible remove the soft constraints ''' if (qpUnfeasible and len(self.softInequalityIndexes) > 0): # remove soft inequality constraints and try to solve again self.removeSoftInequalities = True maxActiveSetIter[0] = maxIter lbAsoft = np.copy(self.lbA) ubAsoft = np.copy(self.ubA) lbAsoft[self.softInequalityIndexes] = -1e100 ubAsoft[self.softInequalityIndexes] = 1e100 imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb, self.ub, lbAsoft, ubAsoft, maxActiveSetIter) self.qpOasesSolver.getPrimalSolution(x) ineq_marg = self.f_inequalities(x) ineq_marg[self.softInequalityIndexes] = 1.0 qpUnfeasible = False if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): ''' if the solution found is unfeasible check whether the initial guess is feasible ''' if (x0 is not None): x = np.copy(x0) ineq_marg = self.f_inequalities(x) ineq_marg[self.softInequalityIndexes] = 1.0 if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): print "[%s] WARNING Problem unfeasible even without soft constraints" % ( self.name), np.min(ineq_marg), imode qpUnfeasible = True elif (self.verb > 0): print "[%s] Initial guess is feasible for the relaxed problem" % ( self.name) else: print "[%s] No way to get a feasible solution (no initial guess)" % ( self.name), np.min(ineq_marg) elif (self.verb > 0): print "[%s] Solution found and initial guess are unfeasible, but relaxed problem is feasible" % ( self.name) if (qpUnfeasible): self.print_qp_oases_error_message(imode, self.name) if (self.verb > 1): activeIneq = np.count_nonzero(np.abs(ineq_marg) < 1e-3) print "[%s] Iter %d, active inequalities %d" % ( self.name, self.iter, activeIneq) # termination conditions if (self.iter >= maxIter): imode = 9 if (self.verb > 1): print "[%s] Max number of iterations reached %d" % ( self.name, self.iter) if (self.qpTime >= maxTime): print "[%s] Max time reached %f after %d iters" % ( self.name, self.qpTime, self.iter) imode = 9 elif (self.solver == 'sqpoases'): ubA = np.array(self.m_in * [1e99]) x_newton = np.zeros(x.shape) x = self.x0 A = self.get_linear_inequality_matrix() self.iter = 0 #total iters of qpoases self.outerIter = 0 # number of outer (Newton) iterations while True: # compute Newton step lb = np.array([b[0] for b in self.bounds]) ub = np.array([b[1] for b in self.bounds]) lb -= x ub -= x lbA = -np.dot(A, x) - self.get_linear_inequality_vector() # if(self.outerIter>0): # if((lbA>0.0).any()): # print "[%s] Iter %d lbA[%d]=%f"%(self.name,self.outerIter,np.argmax(lbA),np.max(lbA)); Hess = self.f_cost_hess(x) grad = self.f_cost_grad(x) self.fx = self.f_cost(x) maxActiveSetIter = np.array([maxIter - self.iter]) maxComputationTime = np.array(maxTime) if (self.initialized == False): imode = self.qpOasesSolver.init(Hess, grad, A, lb, ub, lbA, ubA, maxActiveSetIter, maxComputationTime) if (imode == 0): self.initialized = True else: imode = self.qpOasesSolver.hotstart( Hess, grad, A, lb, ub, lbA, ubA, maxActiveSetIter, maxComputationTime) self.qpTime += maxComputationTime maxTime -= maxComputationTime # count iterations self.iter += 1 + maxActiveSetIter[0] self.outerIter += 1 self.qpOasesSolver.getPrimalSolution(x_newton) ''' check feasibility of the constraints ''' x_new = x + x_newton ineq_marg = self.f_inequalities(x_new) if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): if (len(self.softInequalityIndexes) > 0 and self.outerIter == 1): ''' if constraints are unfeasible at first iteration remove soft inequalities ''' if (self.verb > 1): print '[%s] Remove soft constraints' % (self.name) self.removeSoftInequalities = True self.g[self.softInequalityIndexes] = 1e100 self.iter = 0 continue elif (len(self.softInequalityIndexes) > 0 and self.removeSoftInequalities == False): ''' if constraints are unfeasible at later iteration remove soft inequalities ''' if (self.verb >= 0): print '[%s] Remove soft constraints at iter %d' % ( self.name, self.outerIter) self.removeSoftInequalities = True self.g[self.softInequalityIndexes] = 1e100 continue else: if ((lbA > 0.0).any()): print "[%s] WARNING Problem unfeasible (even without soft constraints) %f" % ( self.name, np.max(lbA)), imode if (self.verb > 0): ''' qp failed for some reason (e.g. max iter) ''' print "[%s] WARNING imode %d ineq unfeasible iter %d: %f, max(lbA)=%f" % ( self.name, imode, self.outerIter, np.min(ineq_marg), np.max(lbA)) break ''' if constraints are unfeasible at later iterations exit ''' if (imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY): print "[%s] Outer iter %d QPoases says problem is unfeasible but constraints are satisfied: %f" % ( self.name, self.outerIter, np.min(ineq_marg)) ind = np.where(ineq_marg < 0.0)[0] self.g[ind] -= ineq_marg[ind] continue self.print_qp_oases_error_message(imode, self.name) # check for convergence newton_dec_squared = np.dot(x_newton, np.dot(Hess, x_newton)) if (0.5 * newton_dec_squared < self.accuracy): ineq_marg = self.f_inequalities(x) if ((ineq_marg >= -self.INEQ_VIOLATION_THR).all()): if (self.verb > 0): print "[%s] Optimization converged in %d steps" % ( self.name, self.iter) break elif (self.verb > 0): v = ineq_marg < -self.INEQ_VIOLATION_THR print( self.name, self.outerIter, "WARNING Solver converged but inequalities violated:", np.where(v), ineq_marg[v]) elif (self.verb > 1): print "[%s] Optimization did not converge yet, squared Newton decrement: %f" % ( self.name, newton_dec_squared) # line search (alpha, fc, gc, phi, old_fval, derphi) = line_search(self.f_cost, self.f_cost_grad, x, x_newton, grad, self.fx, c1=0.0001, c2=0.9) x_new = x + alpha * x_newton new_fx = self.f_cost(x_new) if (self.verb > 1): print "[%s] line search alpha = %f, fc %d, gc %d, old cost %f, new cost %f" % ( self.name, alpha, fc, gc, self.fx, new_fx) # Check that inequalities are still satisfied ineq_marg = self.f_inequalities(x_new) if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()): if (self.verb > 1): print "[%s] WARNING some inequalities are violated with alpha=%f, gonna perform new line search." % ( self.name, alpha) k = 2.0 for i in range(100): alpha = min(k * alpha, 1.0) x_new = x + alpha * x_newton ineq_marg = self.f_inequalities(x_new) if ((ineq_marg >= -self.INEQ_VIOLATION_THR).all()): if (self.verb > 1): print "[%s] With alpha=%f the solution satisfies the inequalities." % ( self.name, alpha) break if (alpha == 1.0): print "[%s] ERROR With alpha=1 some inequalities are violated, error: %f" % ( self.name, np.min(ineq_marg)) break x = x_new if (self.verb > 1): ineq_marg = self.f_inequalities(x) activeIneq = np.count_nonzero(np.abs(ineq_marg) < 1e-3) nViolIneq = np.count_nonzero( ineq_marg < -self.INEQ_VIOLATION_THR) print "[%s] Outer iter %d, iter %d, active inequalities %d, violated inequalities %d" % ( self.name, self.outerIter, self.iter, activeIneq, nViolIneq) # termination conditions if (self.iter >= maxIter): if (self.verb > 1): print "[%s] Max number of iterations reached %d" % ( self.name, self.iter) imode = 9 break if (maxTime < 0.0): print "[%s] Max time reached %.4f s after %d out iters, %d iters, newtonDec %.6f removeSoftIneq" % ( self.name, self.qpTime, self.outerIter, self.iter, newton_dec_squared), self.removeSoftInequalities imode = 9 break self.iterationNumber = self.iter else: print '[%s] Solver type not recognized: %s' % (self.name, self.solver) return np.zeros(self.n) self.computationTime = time.time() - start ineq = self.f_inequalities(x) if (self.removeSoftInequalities): ineq[self.softInequalityIndexes] = 1.0 self.nViolatedInequalities = np.count_nonzero( ineq < -self.INEQ_VIOLATION_THR) self.nActiveInequalities = np.count_nonzero(ineq < 1e-3) self.imode = imode self.print_solution_info(x) self.finalize_solution(x) return (x, imode) def finalize_solution(self, x): pass def f_cost(self, x): e = np.dot(self.D, x) - self.d return 0.5 * np.dot(e.T, e) def f_cost_grad(self, x): return approx_fprime(x, self.f_cost, self.epsilon) def f_cost_hess(self, x): return approx_jacobian(x, self.f_cost_grad, self.epsilon) def get_linear_inequality_matrix(self): return self.A def get_linear_inequality_vectors(self): return (self.lbA, self.ubA) def f_inequalities(self, x): ineq_marg = np.zeros(2 * self.m_in) Ax = np.dot(self.get_linear_inequality_matrix(), x) ineq_marg[:self.m_in] = Ax - self.lbA ineq_marg[self.m_in:] = self.ubA - Ax return ineq_marg def f_inequalities_jac(self, x): return self.get_linear_inequality_matrix() def print_solution_info(self, x): if (self.verb > 1): print(self.name, "Solution is ", x) def reset(self): self.initialized = False def check_grad(self, x=None): if (x is None): x = np.random.rand(self.n) grad = self.f_cost_grad(x) grad_fd = approx_fprime(x, self.f_cost, self.epsilon) err = np.sqrt(sum((grad - grad_fd)**2)) print "[%s] Gradient error: %f" % (self.name, err) return (grad, grad_fd) def check_hess(self, x=None): if (x is None): x = np.random.rand(self.n) hess = self.f_cost_hess(x) hess_fd = approx_jacobian(x, self.f_cost_grad, self.epsilon) err = np.sqrt(np.sum((hess - hess_fd)**2)) print "[%s] Hessian error: %f" % (self.name, err) return (hess, hess_fd) def print_qp_oases_error_message(self, imode, solver_name): if (imode != 0 and imode != PyReturnValue.MAX_NWSR_REACHED): if (imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY): print "[%s] ERROR Qp oases HOTSTART_STOPPED_INFEASIBILITY" % solver_name # 61 elif (imode == PyReturnValue.MAX_NWSR_REACHED): print "[%s] ERROR Qp oases RET_MAX_NWSR_REACHED" % solver_name # 64 elif (imode == PyReturnValue.STEPDIRECTION_FAILED_TQ): print "[%s] ERROR Qp oases STEPDIRECTION_FAILED_TQ" % solver_name # 68 elif (imode == PyReturnValue.STEPDIRECTION_FAILED_CHOLESKY): print "[%s] ERROR Qp oases STEPDIRECTION_FAILED_CHOLESKY" % solver_name # 69 elif (imode == PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED ): print "[%s] ERROR Qp oases HOTSTART_FAILED_AS_QP_NOT_INITIALISED" % solver_name # 54 elif (imode == PyReturnValue.INIT_FAILED_HOTSTART): print "[%s] ERROR Qp oases INIT_FAILED_HOTSTART" % solver_name # 36 elif (imode == PyReturnValue.INIT_FAILED_INFEASIBILITY): print "[%s] ERROR Qp oases INIT_FAILED_INFEASIBILITY" % solver_name # 37 elif (imode == PyReturnValue.UNKNOWN_BUG): print "[%s] ERROR Qp oases UNKNOWN_BUG" % solver_name # 9 else: print "[%s] ERROR Qp oases %d " % (solver_name, imode)
def main(): rospy.init_node('controller_2dof', anonymous=True) rospy.Rate(10) 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
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)
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
class SplineTrajectoryNd(object): def __init__(self, dim, minimal_order, smin, smax): assert dim > 0 and isinstance(dim, int) assert minimal_order >= 2 and isinstance(dim, int) assert smin < smax self.dim = dim self.minimal_order = minimal_order self.support = (float(smin), float(smax)) self.splines = [] self.splines_support = list(self.support) self.way_points = [np.matrix(np.empty([self.dim, 1]))] * 2 self.way_points_constraints = [([None] * (self.minimal_order - 1)), ([None] * (self.minimal_order - 1))] def setInitialWayPoint(self, point): assert point.shape[0] == self.dim self.way_points[0] = np.asmatrix(point).copy() def setFinalWayPoint(self, point): assert point.shape[0] == self.dim self.way_points[-1] = np.asmatrix(point).copy() def numWayPoints(self): return len(self.way_points) def addWayPoint(self, point, s): assert s >= self.support[0] and s <= self.support[1] assert point.shape == (self.dim, 1) if s == self.support[0]: self.setInitialWayPoint(point) id = 0 elif s == self.support[1]: self.setFinalWayPoint(point) id = self.numWayPoints() - 1 elif s in self.splines_support: id = self.splines_support.index(s) self.way_points[id] = point.copy() else: indexes = [ k for k, val in enumerate(self.splines_support) if val >= s ] id = indexes[0] self.way_points.insert(id, point.copy()) self.splines_support.insert(id, s) self.way_points_constraints.insert(id, [None] * (self.minimal_order - 1)) return id def addWayPointConstraint(self, way_point, way_point_constraint, diff_order): assert isinstance(way_point_constraint, WayPointConstraint) vector = way_point_constraint.vector assert vector.shape == (self.dim, 1) assert diff_order >= 1 if isinstance(way_point, int): id = way_point elif isinstance(way_point, np.matrix): assert way_point.shape == (self.dim, 1) try: id = [np.array_equal(way_point, x) for x in self.way_points].index(True) except: raise ValueError('Invalid way_point argument') else: raise ValueError('Invalid way_point argument') constraints = self.way_points_constraints[id] constraint_id = diff_order - 1 if constraint_id < len(constraints): constraints[constraint_id] = copy(way_point_constraint) else: raise ValueError('This case is not handled yet') 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 compute(self): self.splines = [] num_variables = 0 num_equalities = 0 num_inequalities = 0 self.spline_var_dim = [] self.spline_var_pos = [] self.spline_const_dim = [] self.spline_const_pos = [] spline_var_pos = 0 for k in range(self.numWayPoints() - 1): smin = self.splines_support[k] smax = self.splines_support[k + 1] constraints = self.way_points_constraints[k] spline_position_order = 1 # The spline must pass by two points spline_differential_order = 0 # We must ensure the continuity with the next spline if k < self.numWayPoints() - 2: spline_differential_order += self.minimal_order - 1 # In addition, we must add the derivative constraints which concerns the current point spline_additional_differential_order = 0 for i, cons in enumerate(constraints): if cons is not None: spline_additional_differential_order += 1 if cons.type == WayPointConstraint.ConstraintType.FREE: num_variables += 1 # A special care for the last way points if (k == self.numWayPoints() - 2): next_constraints = self.way_points_constraints[-1] for i, cons in enumerate(next_constraints): if cons is not None: spline_additional_differential_order += 1 if cons.type == WayPointConstraint.ConstraintType.FREE: num_variables += 1 spline_differential_order += spline_additional_differential_order spline_order = spline_position_order + spline_differential_order spline_order = max(self.minimal_order, spline_order) spline_var_dim = (spline_order + 1) * self.dim num_variables += spline_var_dim spline_const_dim = (spline_order + 1) * self.dim num_equalities += spline_const_dim # Create spline spline = SplineNd(self.dim, spline_order, smin, smax) spline.setWayPoint(self.way_points[k]) self.splines.append(spline) # Update spline positions and dimensions self.spline_var_dim.append(spline_var_dim) self.spline_var_pos.append(spline_var_pos) spline_var_pos += spline_var_dim # Set up the problem A_eq = np.matrix((np.zeros((num_equalities, num_variables)))) self.A_eq = A_eq b_eq = np.matrix((np.zeros((num_equalities, 1)))) self.b_eq = b_eq A_in = np.matrix((np.zeros((num_inequalities, num_variables)))) self.A_in = A_in b_in = np.matrix((np.zeros((num_inequalities, 1)))) self.b_in = b_in num_splines = len(self.splines) eq_const_id = 0 for k, spline in enumerate(self.splines): spline_var_pos = self.spline_var_pos[k] spline_var_dim = self.spline_var_dim[k] # The spline must pass by two points points = (self.way_points[k], self.way_points[k + 1]) support = spline.support for p_id, point in enumerate(points): var_pos = spline_var_pos for i in range(self.dim): slice = range(var_pos, var_pos + spline.order + 1) res = spline.computeDifferentialConstraint( support[p_id], 0) A_eq[eq_const_id, slice] = res b_eq[eq_const_id] = point[i] var_pos += spline.order + 1 eq_const_id += 1 # The spline has some continuity constraints if k < num_splines - 1: next_spline = self.splines[k + 1] next_spline_var_pos = self.spline_var_pos[k + 1] next_spline_var_dim = self.spline_var_dim[k + 1] s1 = support[1] #continuity_eq_const_id = eq_const_id for diff_order in range(1, self.minimal_order): var_pos = spline_var_pos next_var_pos = next_spline_var_pos for i in range(self.dim): slice = range(var_pos, var_pos + spline.order + 1) res = spline.computeDifferentialConstraint( s1, diff_order) A_eq[eq_const_id, slice] = res next_slice = range( next_var_pos, next_var_pos + next_spline.order + 1) res = next_spline.computeDifferentialConstraint( s1, diff_order) A_eq[eq_const_id, next_slice] = -res b_eq[eq_const_id] = 0. var_pos += spline.order + 1 next_var_pos += next_spline.order + 1 eq_const_id += 1 # The spline may have some initial constraints constraints = self.way_points_constraints[k] for cons_id, cons in enumerate(constraints): if cons is not None: var_pos = spline_var_pos vector = cons.vector for i in range(self.dim): slice = range(var_pos, var_pos + spline.order + 1) res = spline.computeDifferentialConstraint( support[0], cons_id + 1) A_eq[eq_const_id, slice] = res b_eq[eq_const_id] = vector[i] var_pos += spline.order + 1 eq_const_id += 1 # The last spline may have terminal constraints if k == num_splines - 1: constraints = self.way_points_constraints[k + 1] for cons_id, cons in enumerate(constraints): if cons is not None: var_pos = spline_var_pos vector = cons.vector for i in range(self.dim): slice = range(var_pos, var_pos + spline.order + 1) res = spline.computeDifferentialConstraint( support[1], cons_id + 1) A_eq[eq_const_id, slice] = res b_eq[eq_const_id] = vector[i] var_pos += spline.order + 1 eq_const_id += 1 if k < num_splines - 1: next_spline = self.splines[k + 1] next_spline_dim = self.spline_var_dim[k + 1] snext = self.splines_support[k + 1] # Add equality constraint for i in range(self.minimal_order + 1): res = spline.computeDifferentialConstraint(snext, i) sol_naive = np.linalg.pinv(A_eq) * b_eq # Solve with qpOASES for k, spline in enumerate(self.splines): spline_var_pos = self.spline_var_pos[k] spline_var_dim = self.spline_var_dim[k] spline.setCoeffs(sol_naive[spline_var_pos:spline_var_pos + spline_var_dim]) spline.__compute__() self.num_variables = num_variables self.num_equalities = num_equalities self.num_inequalities = num_inequalities def findSpline(self, s): if s <= self.support[0]: id = 0 elif s >= self.support[1]: id = len(self.splines) - 1 else: indexes = [ k for k, val in enumerate(self.splines_support) if s >= val ] id = indexes[-1] return id def eval(self, s): id = self.findSpline(s) assert id >= 0 return self.splines[id].eval(s) def evals(self, s, until_order): id = self.findSpline(s) assert id >= 0 return self.splines[id].evals(s, until_order) def diffEval(self, s, order): id = self.findSpline(s) assert id >= 0 assert order >= 0 return self.splines[id].diffEval(s, order)