def test_Feedback_Linearization_controller_BS(x, t): # Output feedback linearization 2 # # y1= ||r-rf||^2/2 # y2= wx # y3= wy # y4= wz # # Analysis: The zero dynamics is unstable. global g, xf, Aout, Bout, Mout, T_period, w_freq, radius print("%%%%%%%%%%%%%%%%%%%%%") print("%%CLF-QP %%%%%%%%%%%") print("%%%%%%%%%%%%%%%%%%%%%") print("t:", t) prog = MathematicalProgram() u_var = prog.NewContinuousVariables(4, "u_var") c_var = prog.NewContinuousVariables(1, "c_var") # # # Example 1 : Circle x_f = radius * math.cos(w_freq * t) y_f = radius * math.sin(w_freq * t) # print("x_f:",x_f) # print("y_f:",y_f) dx_f = -radius * math.pow(w_freq, 1) * math.sin(w_freq * t) dy_f = radius * math.pow(w_freq, 1) * math.cos(w_freq * t) ddx_f = -radius * math.pow(w_freq, 2) * math.cos(w_freq * t) ddy_f = -radius * math.pow(w_freq, 2) * math.sin(w_freq * t) dddx_f = radius * math.pow(w_freq, 3) * math.sin(w_freq * t) dddy_f = -radius * math.pow(w_freq, 3) * math.cos(w_freq * t) ddddx_f = radius * math.pow(w_freq, 4) * math.cos(w_freq * t) ddddy_f = radius * math.pow(w_freq, 4) * math.sin(w_freq * t) # # Example 2 : Lissajous curve a=1 b=2 # ratio_ab=2; # a=1; # b=ratio_ab*a; # delta_lissajous = 0 #math.pi/2; # x_f = radius*math.sin(a*w_freq*t+delta_lissajous) # y_f = radius*math.sin(b*w_freq*t) # # print("x_f:",x_f) # # print("y_f:",y_f) # dx_f = radius*math.pow(a*w_freq,1)*math.cos(a*w_freq*t+delta_lissajous) # dy_f = radius*math.pow(b*w_freq,1)*math.cos(b*w_freq*t) # ddx_f = -radius*math.pow(a*w_freq,2)*math.sin(a*w_freq*t+delta_lissajous) # ddy_f = -radius*math.pow(b*w_freq,2)*math.sin(b*w_freq*t) # dddx_f = -radius*math.pow(a*w_freq,3)*math.cos(a*w_freq*t+delta_lissajous) # dddy_f = -radius*math.pow(b*w_freq,3)*math.cos(b*w_freq*t) # ddddx_f = radius*math.pow(a*w_freq,4)*math.sin(a*w_freq*t+delta_lissajous) # ddddy_f = radius*math.pow(b*w_freq,4)*math.sin(b*w_freq*t) e1 = np.array([1, 0, 0]) # e3 elementary vector e2 = np.array([0, 1, 0]) # e3 elementary vector e3 = np.array([0, 0, 1]) # e3 elementary vector # epsilonn= 1e-0 # alpha = 100; # kp1234=alpha*1/math.pow(epsilonn,4) # gain for y # kd1=4/math.pow(epsilonn,3) # gain for y^(1) # kd2=12/math.pow(epsilonn,2) # gain for y^(2) # kd3=4/math.pow(epsilonn,1) # gain for y^(3) # kp5= 10; # gain for y5 q = np.zeros(7) qd = np.zeros(6) q = x[0:8] qd = x[8:15] print("qnorm:", np.dot(q[3:7], q[3:7])) q0 = q[3] q1 = q[4] q2 = q[5] q3 = q[6] xi1 = q[7] v = qd[0:3] w = qd[3:6] xi2 = qd[6] xd = xf[0] yd = xf[1] zd = xf[2] wd = xf[11:14] # Useful vectors and matrices (Rq, Eq, wIw, I_inv) = robobee_plantBS.GetManipulatorDynamics(q, qd) F1q = np.zeros((3, 4)) F1q[0, :] = np.array([q2, q3, q0, q1]) F1q[1, :] = np.array([-1 * q1, -1 * q0, q3, q2]) F1q[2, :] = np.array([q0, -1 * q1, -1 * q2, q3]) Rqe3 = np.dot(Rq, e3) Rqe3_hat = np.zeros((3, 3)) Rqe3_hat[0, :] = np.array([0, -Rqe3[2], Rqe3[1]]) Rqe3_hat[1, :] = np.array([Rqe3[2], 0, -Rqe3[0]]) Rqe3_hat[2, :] = np.array([-Rqe3[1], Rqe3[0], 0]) Rqe1 = np.dot(Rq, e1) Rqe1_hat = np.zeros((3, 3)) Rqe1_hat[0, :] = np.array([0, -Rqe1[2], Rqe1[1]]) Rqe1_hat[1, :] = np.array([Rqe1[2], 0, -Rqe1[0]]) Rqe1_hat[2, :] = np.array([-Rqe1[1], Rqe1[0], 0]) Rqe1_x = np.dot(e2.T, Rqe1) Rqe1_y = np.dot(e1.T, Rqe1) w_hat = np.zeros((3, 3)) w_hat[0, :] = np.array([0, -w[2], w[1]]) w_hat[1, :] = np.array([w[2], 0, -w[0]]) w_hat[2, :] = np.array([-w[1], w[0], 0]) Rw = np.dot(Rq, w) Rw_hat = np.zeros((3, 3)) Rw_hat[0, :] = np.array([0, -Rw[2], Rw[1]]) Rw_hat[1, :] = np.array([Rw[2], 0, -Rw[0]]) Rw_hat[2, :] = np.array([-Rw[1], Rw[0], 0]) #- Checking the derivation # print("F1qEqT-(-Rqe3_hat)",np.dot(F1q,Eq.T)-(-Rqe3_hat)) # Rqe3_cal = np.zeros(3) # Rqe3_cal[0] = 2*(q3*q1+q0*q2) # Rqe3_cal[1] = 2*(q3*q2-q0*q1) # Rqe3_cal[2] = (q0*q0+q3*q3-q1*q1-q2*q2) # print("Rqe3 - Rqe3_cal", Rqe3-Rqe3_cal) # Four output y1 = x[0] - x_f y2 = x[1] - y_f y3 = x[2] - zd - 0 y4 = math.atan2(Rqe1_x, Rqe1_y) - math.pi / 8 # print("Rqe1_x:", Rqe1_x) eta1 = np.zeros(3) eta1 = np.array([y1, y2, y3]) eta5 = y4 # print("y4", y4) # First derivative of first three output and yaw output eta2 = np.zeros(3) eta2 = v - np.array([dx_f, dy_f, 0]) dy1 = eta2[0] dy2 = eta2[1] dy3 = eta2[2] x2y2 = (math.pow(Rqe1_x, 2) + math.pow(Rqe1_y, 2)) # x^2+y^2 eta6_temp = np.zeros(3) #eta6_temp = (ye2T-xe1T)/(x^2+y^2) eta6_temp = (Rqe1_y * e2.T - Rqe1_x * e1.T) / x2y2 # print("eta6_temp:", eta6_temp) # Body frame w ( multiply R) eta6 = np.dot(eta6_temp, np.dot(-Rqe1_hat, np.dot(Rq, w))) # World frame w # eta6 = np.dot(eta6_temp,np.dot(-Rqe1_hat,w)) # print("Rqe1_hat:", Rqe1_hat) # Second derivative of first three output eta3 = np.zeros(3) eta3 = -g * e3 + Rqe3 * xi1 - np.array([ddx_f, ddy_f, 0]) ddy1 = eta3[0] ddy2 = eta3[1] ddy3 = eta3[2] # Third derivative of first three output eta4 = np.zeros(3) # Body frame w ( multiply R) eta4 = Rqe3 * xi2 + np.dot(-Rqe3_hat, np.dot(Rq, w)) * xi1 - np.array( [dddx_f, dddy_f, 0]) # World frame w # eta4 = Rqe3*xi2+np.dot(np.dot(F1q,Eq.T),w)*xi1 - np.array([dddx_f,dddy_f,0]) dddy1 = eta4[0] dddy2 = eta4[1] dddy3 = eta4[2] # Fourth derivative of first three output B_qw_temp = np.zeros(3) # Body frame w B_qw_temp = xi1 * (-np.dot(Rw_hat, np.dot(Rqe3_hat, Rw)) + np.dot(Rqe3_hat, np.dot(Rq, np.dot(I_inv, wIw))) ) # np.dot(I_inv,wIw)*xi1-2*w*xi2 B_qw = B_qw_temp + xi2 * (-2 * np.dot(Rqe3_hat, Rw)) - np.array( [ddddx_f, ddddy_f, 0]) #np.dot(Rqe3_hat,B_qw_temp) # World frame w # B_qw_temp = xi1*(-np.dot(w_hat,np.dot(Rqe3_hat,w))+np.dot(Rqe3_hat,np.dot(I_inv,wIw))) # np.dot(I_inv,wIw)*xi1-2*w*xi2 # B_qw = B_qw_temp+xi2*(-2*np.dot(Rqe3_hat,w)) - np.array([ddddx_f,ddddy_f,0]) #np.dot(Rqe3_hat,B_qw_temp) # B_qw = B_qw_temp - np.dot(w_hat,np.dot(Rqe3_hat,w))*xi1 # Second derivative of yaw output # Body frame w dRqe1_x = np.dot(e2, np.dot(-Rqe1_hat, Rw)) # \dot{x} dRqe1_y = np.dot(e1, np.dot(-Rqe1_hat, Rw)) # \dot{y} alpha1 = 2 * (Rqe1_x * dRqe1_x + Rqe1_y * dRqe1_y) / x2y2 # (2xdx +2ydy)/(x^2+y^2) # World frame w # dRqe1_x = np.dot(e2,np.dot(-Rqe1_hat,w)) # \dot{x} # dRqe1_y = np.dot(e1,np.dot(-Rqe1_hat,w)) # \dot{y} # alpha1 = 2*(Rqe1_x*dRqe1_x+Rqe1_y*dRqe1_y)/x2y2 # (2xdx +2ydy)/(x^2+y^2) # # alpha2 = math.pow(dRqe1_y,2)-math.pow(dRqe1_x,2) # Body frame w B_yaw_temp3 = np.zeros(3) B_yaw_temp3 = alpha1 * np.dot(Rqe1_hat, Rw) + np.dot( Rqe1_hat, np.dot(Rq, np.dot(I_inv, wIw))) - np.dot( Rw_hat, np.dot(Rqe1_hat, Rw)) B_yaw = np.dot(eta6_temp, B_yaw_temp3) # +alpha2 :Could be an error in math. g_yaw = np.zeros(3) g_yaw = -np.dot(eta6_temp, np.dot(Rqe1_hat, np.dot(Rq, I_inv))) # World frame w # B_yaw_temp3 =np.zeros(3) # B_yaw_temp3 = alpha1*np.dot(Rqe1_hat,w)+np.dot(Rqe1_hat,np.dot(I_inv,wIw))-np.dot(w_hat,np.dot(Rqe1_hat,w)) # B_yaw = np.dot(eta6_temp,B_yaw_temp3) # +alpha2 :Could be an error in math. # g_yaw = np.zeros(3) # g_yaw = -np.dot(eta6_temp,np.dot(Rqe1_hat,I_inv)) # print("g_yaw:", g_yaw) # Decoupling matrix A(x)\in\mathbb{R}^4 A_fl = np.zeros((4, 4)) A_fl[0:3, 0] = Rqe3 # Body frame w A_fl[0:3, 1:4] = -np.dot(Rqe3_hat, np.dot(Rq, I_inv)) * xi1 # World frame w # A_fl[0:3,1:4] = -np.dot(Rqe3_hat,I_inv)*xi1 A_fl[3, 1:4] = g_yaw A_fl_inv = np.linalg.inv(A_fl) A_fl_det = np.linalg.det(A_fl) # print("I_inv:", I_inv) print("A_fl:", A_fl) print("A_fl_det:", A_fl_det) # Drift in the output dynamics U_temp = np.zeros(4) U_temp[0:3] = B_qw U_temp[3] = B_yaw # Output dyamics eta = np.hstack([eta1, eta2, eta3, eta5, eta4, eta6]) eta_norm = np.dot(eta, eta) # v-CLF QP controller FP_PF = np.dot(Aout.T, Mout) + np.dot(Mout, Aout) PG = np.dot(Mout, Bout) L_FVx = np.dot(eta, np.dot(FP_PF, eta)) L_GVx = 2 * np.dot(eta.T, PG) # row vector L_fhx_star = U_temp Vx = np.dot(eta, np.dot(Mout, eta)) # phi0_exp = L_FVx+np.dot(L_GVx,L_fhx_star)+(min_e_Q/max_e_P)*Vx*1 # exponentially stabilizing # phi0_exp = L_FVx+np.dot(L_GVx,L_fhx_star)+min_e_Q*eta_norm # more exact bound - exponentially stabilizing phi0_exp = L_FVx + np.dot( L_GVx, L_fhx_star) # more exact bound - exponentially stabilizing phi1_decouple = np.dot(L_GVx, A_fl) # # Solve QP v_var = np.dot(A_fl, u_var) + L_fhx_star Quadratic_Positive_def = np.matmul(A_fl.T, A_fl) QP_det = np.linalg.det(Quadratic_Positive_def) c_QP = 2 * np.dot(L_fhx_star.T, A_fl) c_QP_extned = np.hstack([c_QP, -1]) u_var_extended = np.hstack([u_var, c_var]) # CLF_QP_cost_v = np.dot(v_var,v_var) // Exact quadratic cost CLF_QP_cost_v_effective = np.dot( u_var, np.dot(Quadratic_Positive_def, u_var)) + np.dot( c_QP, u_var) - c_var[0] # Quadratic cost without constant term # CLF_QP_cost_u = np.dot(u_var,u_var) phi1 = np.dot(phi1_decouple, u_var) + c_var[0] * eta_norm #----Printing intermediate states # print("L_fhx_star: ",L_fhx_star) # print("c_QP:", c_QP) # print("Qp : ",Quadratic_Positive_def) # print("Qp det: ", QP_det) # print("c_QP", c_QP) # print("phi0_exp: ", phi0_exp) # print("PG:", PG) # print("L_GVx:", L_GVx) # print("eta6", eta6) # print("d : ", phi1_decouple) # print("Cost expression:", CLF_QP_cost_v) #print("Const expression:", phi0_exp+phi1) #----Different solver option // Gurobi did not work with python at this point (some binding issue 8/8/2018) solver = IpoptSolver() # solver = GurobiSolver() # print solver.available() # assert(solver.available()==True) # assertEqual(solver.solver_type(), mp.SolverType.kGurobi) # solver.Solve(prog) # assertEqual(result, mp.SolutionResult.kSolutionFound) # mp.AddLinearConstraint() # print("x:", x) # print("phi_0_exp:", phi0_exp) # print("phi1_decouple:", phi1_decouple) # print("eta1:", eta1) # print("eta2:", eta2) # print("eta3:", eta3) # print("eta4:", eta4) # print("eta5:", eta5) # print("eta6:", eta6) # Output Feedback Linearization controller mu = np.zeros(4) k = np.zeros((4, 14)) k = np.matmul(Bout.T, Mout) k = np.matmul(np.linalg.inv(R), k) mu = -1 / 1 * np.matmul(k, eta) v = -U_temp + mu U_fl = np.matmul( A_fl_inv, v ) # Output Feedback controller to comare the result with CLF-QP solver # Set up the QP problem prog.AddQuadraticCost(CLF_QP_cost_v_effective) prog.AddConstraint(phi0_exp + phi1 <= 0) prog.AddConstraint(c_var[0] >= 0) prog.AddConstraint(c_var[0] <= 100) prog.AddConstraint(u_var[0] <= 30) prog.SetSolverOption(mp.SolverType.kIpopt, "print_level", 5) # CAUTION: Assuming that solver used Ipopt print("CLF value:", Vx) # Current CLF value prog.SetInitialGuess(u_var, U_fl) prog.Solve() # Solve with default osqp # solver.Solve(prog) print("Optimal u : ", prog.GetSolution(u_var)) U_CLF_QP = prog.GetSolution(u_var) C_CLF_QP = prog.GetSolution(c_var) #---- Printing for debugging # dx = robobee_plantBS.evaluate_f(U_fl,x) # print("dx", dx) # print("\n######################") # # print("qe3:", A_fl[0,0]) # print("u:", u) # print("\n####################33") # deta4 = B_qw+Rqe3*U_fl_zero[0]+np.matmul(-np.matmul(Rqe3_hat,I_inv),U_fl_zero[1:4])*xi1 # deta6 = B_yaw+np.dot(g_yaw,U_fl_zero[1:4]) # print("deta4:",deta4) # print("deta6:",deta6) print(C_CLF_QP) phi1_opt = np.dot(phi1_decouple, U_CLF_QP) + C_CLF_QP * eta_norm phi1_opt_FL = np.dot(phi1_decouple, U_fl) print("FL u: ", U_fl) print("CLF u:", U_CLF_QP) print("Cost FL: ", np.dot(mu, mu)) v_CLF = np.dot(A_fl, U_CLF_QP) + L_fhx_star print("Cost CLF: ", np.dot(v_CLF, v_CLF)) print("Constraint FL : ", phi0_exp + phi1_opt_FL) print("Constraint CLF : ", phi0_exp + phi1_opt) u = U_CLF_QP print("eigenvalues minQ maxP:", [min_e_Q, max_e_P]) return u
def __init__(self, rtree, q_nom, control_period=0.001): self.rtree = rtree self.nq = rtree.get_num_positions() self.nv = rtree.get_num_velocities() self.nu = rtree.get_num_actuators() dim = 3 # 3D nd = 4 # for friction cone approx, hard coded for now self.nc = 4 # number of contacts; TODO don't hard code self.nf = self.nc * nd # number of contact force variables self.ncf = 2 # number of loop constraint forces; TODO don't hard code self.neps = self.nc * dim # number of slack variables for contact self.q_nom = q_nom self.com_des = rtree.centerOfMass(self.rtree.doKinematics(q_nom)) self.u_des = CassieFixedPointTorque() self.lfoot = rtree.FindBody("toe_left") self.rfoot = rtree.FindBody("toe_right") self.springs = 2 + np.array([ rtree.FindIndexOfChildBodyOfJoint("knee_spring_left_fixed"), rtree.FindIndexOfChildBodyOfJoint("knee_spring_right_fixed"), rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_left"), rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_right") ]) umin = np.zeros(self.nu) umax = np.zeros(self.nu) ii = 0 for motor in rtree.actuators: umin[ii] = motor.effort_limit_min umax[ii] = motor.effort_limit_max ii += 1 slack_limit = 10.0 self.initialized = False #------------------------------------------------------------ # Add Decision Variables ------------------------------------ prog = MathematicalProgram() qddot = prog.NewContinuousVariables(self.nq, "joint acceleration") u = prog.NewContinuousVariables(self.nu, "input") bar = prog.NewContinuousVariables(self.ncf, "four bar forces") beta = prog.NewContinuousVariables(self.nf, "friction forces") eps = prog.NewContinuousVariables(self.neps, "slack variable") nparams = prog.num_vars() #------------------------------------------------------------ # Problem Constraints --------------------------------------- self.con_u_lim = prog.AddBoundingBoxConstraint(umin, umax, u).evaluator() self.con_fric_lim = prog.AddBoundingBoxConstraint(0, 100000, beta).evaluator() self.con_slack_lim = prog.AddBoundingBoxConstraint( -slack_limit, slack_limit, eps).evaluator() bar_con = np.zeros((self.ncf, self.nq)) self.con_4bar = prog.AddLinearEqualityConstraint( bar_con, np.zeros(self.ncf), qddot).evaluator() if self.nc > 0: dyn_con = np.zeros( (self.nq, self.nq + self.nu + self.ncf + self.nf)) dyn_vars = np.concatenate((qddot, u, bar, beta)) self.con_dyn = prog.AddLinearEqualityConstraint( dyn_con, np.zeros(self.nq), dyn_vars).evaluator() foot_con = np.zeros((self.neps, self.nq + self.neps)) foot_vars = np.concatenate((qddot, eps)) self.con_foot = prog.AddLinearEqualityConstraint( foot_con, np.zeros(self.neps), foot_vars).evaluator() else: dyn_con = np.zeros((self.nq, self.nq + self.nu + self.ncf)) dyn_vars = np.concatenate((qddot, u, bar)) self.con_dyn = prog.AddLinearEqualityConstraint( dyn_con, np.zeros(self.nq), dyn_vars).evaluator() #------------------------------------------------------------ # Problem Costs --------------------------------------------- self.Kp_com = 50 self.Kd_com = 2.0 * sqrt(self.Kp_com) # self.Kp_qdd = 10*np.eye(self.nq)#np.diag(np.diag(H)/np.max(np.diag(H))) self.Kp_qdd = np.diag( np.concatenate(([10, 10, 10, 5, 5, 5], np.zeros(self.nq - 6)))) self.Kd_qdd = 1.0 * np.sqrt(self.Kp_qdd) self.Kp_qdd[self.springs, self.springs] = 0 self.Kd_qdd[self.springs, self.springs] = 0 com_ddot_des = np.zeros(dim) qddot_des = np.zeros(self.nq) self.w_com = 0 self.w_qdd = np.zeros(self.nq) self.w_qdd[:6] = 10 self.w_qdd[8:10] = 1 self.w_qdd[3:6] = 1000 # self.w_qdd[self.springs] = 0 self.w_u = 0.001 * np.ones(self.nu) self.w_u[2:4] = 0.01 self.w_slack = 0.1 self.cost_qdd = prog.AddQuadraticErrorCost(np.diag(self.w_qdd), qddot_des, qddot).evaluator() self.cost_u = prog.AddQuadraticErrorCost(np.diag(self.w_u), self.u_des, u).evaluator() self.cost_slack = prog.AddQuadraticErrorCost( self.w_slack * np.eye(self.neps), np.zeros(self.neps), eps).evaluator() # self.cost_com = prog.AddQuadraticCost().evaluator() # self.cost_qdd = prog.AddQuadraticCost( # 2*np.diag(self.w_qdd), -2*np.matmul(qddot_des, np.diag(self.w_qdd)), qddot).evaluator() # self.cost_u = prog.AddQuadraticCost( # 2*np.diag(self.w_u), -2*np.matmul(self.u_des, np.diag(self.w_u)) , u).evaluator() # self.cost_slack = prog.AddQuadraticCost( # 2*self.w_slack*np.eye(self.neps), np.zeros(self.neps), eps).evaluator() REG = 1e-8 self.cost_reg = prog.AddQuadraticErrorCost( REG * np.eye(nparams), np.zeros(nparams), prog.decision_variables()).evaluator() # self.cost_reg = prog.AddQuadraticCost(2*REG*np.eye(nparams), # np.zeros(nparams), prog.decision_variables()).evaluator() #------------------------------------------------------------ # Solver settings ------------------------------------------- self.solver = GurobiSolver() # self.solver = OsqpSolver() prog.SetSolverOption(SolverType.kGurobi, "Method", 2) #------------------------------------------------------------ # Save MathProg Variables ----------------------------------- self.qddot = qddot self.u = u self.bar = bar self.beta = beta self.eps = eps self.prog = prog
eval_Q = np.linalg.eigvals(Q) eval_P = np.linalg.eigvals(Mout) min_e_Q = np.min(eval_Q) max_e_P = np.max(eval_P) print("Evalues: ", [min_e_Q, max_e_P]) # Set up for QP problem prog = MathematicalProgram() u_var = prog.NewContinuousVariables(4, "u_var") c_var = prog.NewContinuousVariables(1, "c_var") solverid = prog.GetSolverId() tol = 1e-10 prog.SetSolverOption(mp.SolverType.kIpopt, "tol", tol) prog.SetSolverOption(mp.SolverType.kIpopt, "constr_viol_tol", tol) prog.SetSolverOption(mp.SolverType.kIpopt, "acceptable_tol", tol) prog.SetSolverOption(mp.SolverType.kIpopt, "acceptable_constr_viol_tol", tol) prog.SetSolverOption(mp.SolverType.kIpopt, "print_level", 5) # CAUTION: Assuming that solver used Ipopt # #-[2] Linearization and get (K,S) for LQR # A, B =robobee_plantBS.GetLinearizedDynamics(uf, xf) # print("A:", A, "B:", B) # ControllabilityMatrix = np.zeros((15, 4*15)) # for i in range(0,15):
def quadratic_program(H, f, A, b, C=None, d=None, tol=1.e-5, **kwargs): """ Solves the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. Arguments ---------- H : numpy.ndarray Positive definite Hessian of the cost function. f : numpy.ndarray Gradient of the cost function. A : numpy.ndarray Left-hand side of the inequality constraints. b : numpy.ndarray Right-hand side of the inequality constraints. C : numpy.ndarray Left-hand side of the equality constraints. d : numpy.ndarray Right-hand side of the equality constraints. tol : float Maximum value of a residual of an inequality to consider the related constraint active. Returns ---------- sol : dict Dictionary with the solution of the QP. Fields ---------- min : float Minimum of the QP (None if the problem is unfeasible). argmin : numpy.ndarray Argument that minimizes the QP (None if the problem is unfeasible). active_set : list of int Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible). multiplier_inequality : numpy.ndarray Lagrange multipliers for the inequality constraints (None if the problem is unfeasible). multiplier_equality : numpy.ndarray Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints). """ # check equalities if (C is None) != (d is None): raise ValueError('missing C or d.') # problem size n_ineq, n_x = A.shape if C is not None: n_eq = C.shape[0] else: n_eq = 0 # build program prog = MathematicalProgram() x = prog.NewContinuousVariables(n_x) [prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)] [prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)] inequalities = [] prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x)) # solve solver = GurobiSolver() prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0) [prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()] result = prog.Solve() # initialize output sol = { 'min': None, 'argmin': None, 'active_set': None, 'multiplier_inequality': None, 'multiplier_equality': None } if result == SolutionResult.kSolutionFound: sol['argmin'] = prog.GetSolution(x) sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin']) sol['active_set'] = np.where(A.dot(sol['argmin']) - b > -tol)[0].tolist() # retrieve multipliers through KKT conditions lhs = A[sol['active_set']] rhs = b[sol['active_set']] if n_eq > 0: lhs = np.vstack((lhs, C)) rhs = np.concatenate((rhs, d)) H_inv = np.linalg.inv(H) M = lhs.dot(H_inv).dot(lhs.T) m = - np.linalg.inv(M).dot(lhs.dot(H_inv).dot(f) + rhs) sol['multiplier_inequality'] = np.zeros(n_ineq) for i, j in enumerate(sol['active_set']): sol['multiplier_inequality'][j] = m[i] if n_eq > 0: sol['multiplier_equality'] = m[len(sol['active_set']):] return sol
def __init__(self, rtree, q_nom, control_period=0.001, print_period=1.0, sim=True, cost_pub=False): LeafSystem.__init__(self) self.rtree = rtree self.nq = rtree.get_num_positions() self.nv = rtree.get_num_velocities() self.nu = rtree.get_num_actuators() self.cost_pub = cost_pub dim = 3 # 3D nd = 4 # for friction cone approx, hard coded for now self.nc = 4 # number of contacts; TODO don't hard code self.nf = self.nc * nd # number of contact force variables self.ncf = 2 # number of loop constraint forces; TODO don't hard code self.neps = self.nc * dim # number of slack variables for contact if sim: self.sim = True self._DeclareInputPort(PortDataType.kVectorValued, self.nq + self.nv) self._DeclareDiscreteState(self.nu) self._DeclareVectorOutputPort(BasicVector(self.nu), self.CopyStateOutSim) self.print_period = print_period self.last_print_time = -print_period self.last_v = np.zeros(3) self.lcmgl = lcmgl("Balancing-plan", true_lcm.LCM()) # self.lcmgl = None else: self.sim = False self._DeclareInputPort(PortDataType.kVectorValued, 1) self._DeclareInputPort(PortDataType.kVectorValued, kCassiePositions) self._DeclareInputPort(PortDataType.kVectorValued, kCassieVelocities) self._DeclareInputPort(PortDataType.kVectorValued, 2) self._DeclareDiscreteState(kCassieActuators * 8 + 1) self._DeclareVectorOutputPort( BasicVector(1), lambda c, o: self.CopyStateOut(c, o, 8 * kCassieActuators, 1)) self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 0, kCassieActuators)) #torque self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, kCassieActuators, kCassieActuators)) #motor_pos self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 2 * kCassieActuators, kCassieActuators)) #motor_vel self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 3 * kCassieActuators, kCassieActuators)) #kp self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 4 * kCassieActuators, kCassieActuators)) #kd self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 5 * kCassieActuators, kCassieActuators)) #ki self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 6 * kCassieActuators, kCassieActuators)) #leak self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 7 * kCassieActuators, kCassieActuators)) #clamp self._DeclarePeriodicDiscreteUpdate(0.0005) self.q_nom = q_nom self.com_des = rtree.centerOfMass(self.rtree.doKinematics(q_nom)) self.u_des = CassieFixedPointTorque() self.lfoot = rtree.FindBody("toe_left") self.rfoot = rtree.FindBody("toe_right") self.springs = 2 + np.array([ rtree.FindIndexOfChildBodyOfJoint("knee_spring_left_fixed"), rtree.FindIndexOfChildBodyOfJoint("knee_spring_right_fixed"), rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_left"), rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_right") ]) umin = np.zeros(self.nu) umax = np.zeros(self.nu) ii = 0 for motor in rtree.actuators: umin[ii] = motor.effort_limit_min umax[ii] = motor.effort_limit_max ii += 1 slack_limit = 10.0 self.initialized = False #------------------------------------------------------------ # Add Decision Variables ------------------------------------ prog = MathematicalProgram() qddot = prog.NewContinuousVariables(self.nq, "joint acceleration") u = prog.NewContinuousVariables(self.nu, "input") bar = prog.NewContinuousVariables(self.ncf, "four bar forces") beta = prog.NewContinuousVariables(self.nf, "friction forces") eps = prog.NewContinuousVariables(self.neps, "slack variable") nparams = prog.num_vars() #------------------------------------------------------------ # Problem Constraints --------------------------------------- self.con_u_lim = prog.AddBoundingBoxConstraint(umin, umax, u).evaluator() self.con_fric_lim = prog.AddBoundingBoxConstraint(0, 100000, beta).evaluator() self.con_slack_lim = prog.AddBoundingBoxConstraint( -slack_limit, slack_limit, eps).evaluator() bar_con = np.zeros((self.ncf, self.nq)) self.con_4bar = prog.AddLinearEqualityConstraint( bar_con, np.zeros(self.ncf), qddot).evaluator() if self.nc > 0: dyn_con = np.zeros( (self.nq, self.nq + self.nu + self.ncf + self.nf)) dyn_vars = np.concatenate((qddot, u, bar, beta)) self.con_dyn = prog.AddLinearEqualityConstraint( dyn_con, np.zeros(self.nq), dyn_vars).evaluator() foot_con = np.zeros((self.neps, self.nq + self.neps)) foot_vars = np.concatenate((qddot, eps)) self.con_foot = prog.AddLinearEqualityConstraint( foot_con, np.zeros(self.neps), foot_vars).evaluator() else: dyn_con = np.zeros((self.nq, self.nq + self.nu + self.ncf)) dyn_vars = np.concatenate((qddot, u, bar)) self.con_dyn = prog.AddLinearEqualityConstraint( dyn_con, np.zeros(self.nq), dyn_vars).evaluator() #------------------------------------------------------------ # Problem Costs --------------------------------------------- self.Kp_com = 50 self.Kd_com = 2.0 * sqrt(self.Kp_com) # self.Kp_qdd = 10*np.eye(self.nq)#np.diag(np.diag(H)/np.max(np.diag(H))) self.Kp_qdd = np.diag( np.concatenate(([10, 10, 10, 5, 5, 5], np.zeros(self.nq - 6)))) self.Kd_qdd = 1.0 * np.sqrt(self.Kp_qdd) self.Kp_qdd[self.springs, self.springs] = 0 self.Kd_qdd[self.springs, self.springs] = 0 com_ddot_des = np.zeros(dim) qddot_des = np.zeros(self.nq) self.w_com = 0 self.w_qdd = np.zeros(self.nq) self.w_qdd[:6] = 10 self.w_qdd[8:10] = 1 self.w_qdd[3:6] = 1000 # self.w_qdd[self.springs] = 0 self.w_u = 0.001 * np.ones(self.nu) self.w_u[2:4] = 0.01 self.w_slack = 0.1 self.cost_qdd = prog.AddQuadraticErrorCost(np.diag(self.w_qdd), qddot_des, qddot).evaluator() self.cost_u = prog.AddQuadraticErrorCost(np.diag(self.w_u), self.u_des, u).evaluator() self.cost_slack = prog.AddQuadraticErrorCost( self.w_slack * np.eye(self.neps), np.zeros(self.neps), eps).evaluator() # self.cost_com = prog.AddQuadraticCost().evaluator() # self.cost_qdd = prog.AddQuadraticCost( # 2*np.diag(self.w_qdd), -2*np.matmul(qddot_des, np.diag(self.w_qdd)), qddot).evaluator() # self.cost_u = prog.AddQuadraticCost( # 2*np.diag(self.w_u), -2*np.matmul(self.u_des, np.diag(self.w_u)) , u).evaluator() # self.cost_slack = prog.AddQuadraticCost( # 2*self.w_slack*np.eye(self.neps), np.zeros(self.neps), eps).evaluator() REG = 1e-8 self.cost_reg = prog.AddQuadraticErrorCost( REG * np.eye(nparams), np.zeros(nparams), prog.decision_variables()).evaluator() # self.cost_reg = prog.AddQuadraticCost(2*REG*np.eye(nparams), # np.zeros(nparams), prog.decision_variables()).evaluator() #------------------------------------------------------------ # Solver settings ------------------------------------------- self.solver = GurobiSolver() # self.solver = OsqpSolver() prog.SetSolverOption(SolverType.kGurobi, "Method", 2) #------------------------------------------------------------ # Save MathProg Variables ----------------------------------- self.qddot = qddot self.u = u self.bar = bar self.beta = beta self.eps = eps self.prog = prog
def mixed_integer_quadratic_program(nc, H, f, A, b, C=None, d=None, **kwargs): """ Solves the strictly convex (H > 0) mixed-integer quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. The first nc variables in x are continuous, the remaining are binaries. Arguments ---------- nc : int Number of continuous variables in the problem. H : numpy.ndarray Positive definite Hessian of the cost function. f : numpy.ndarray Gradient of the cost function. A : numpy.ndarray Left-hand side of the inequality constraints. b : numpy.ndarray Right-hand side of the inequality constraints. C : numpy.ndarray Left-hand side of the equality constraints. d : numpy.ndarray Right-hand side of the equality constraints. Returns ---------- sol : dict Dictionary with the solution of the MIQP. Fields ---------- min : float Minimum of the MIQP (None if the problem is unfeasible). argmin : numpy.ndarray Argument that minimizes the MIQP (None if the problem is unfeasible). """ # check equalities if (C is None) != (d is None): raise ValueError('missing C or d.') # problem size n_ineq, n_x = A.shape if C is not None: n_eq = C.shape[0] else: n_eq = 0 # build program prog = MathematicalProgram() x = np.hstack(( prog.NewContinuousVariables(nc), prog.NewBinaryVariables(n_x - nc) )) [prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)] [prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)] prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x)) # solve solver = GurobiSolver() prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0) [prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()] result = prog.Solve() # initialize output sol = { 'min': None, 'argmin': None } if result == SolutionResult.kSolutionFound: sol['argmin'] = prog.GetSolution(x) sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin']) return sol
def linear_program(f, A, b, C=None, d=None, tol=1.e-5): """ Solves the linear program min_x f^T x s.t. A x <= b, C x = d. Arguments ---------- f : numpy.ndarray Gradient of the cost function. A : numpy.ndarray Left-hand side of the inequality constraints. b : numpy.ndarray Right-hand side of the inequality constraints. C : numpy.ndarray Left-hand side of the equality constraints. d : numpy.ndarray Right-hand side of the equality constraints. tol : float Maximum value of a residual of an inequality to consider the related constraint active. Returns ---------- sol : dict Dictionary with the solution of the LP. Fields ---------- min : float Minimum of the LP (None if the problem is unfeasible or unbounded). argmin : numpy.ndarray Argument that minimizes the LP (None if the problem is unfeasible or unbounded). active_set : list of int Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded). multiplier_inequality : numpy.ndarray Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded). multiplier_equality : numpy.ndarray Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints). """ # check equalities if (C is None) != (d is None): raise ValueError('missing C or d.') # problem size n_ineq, n_x = A.shape if C is not None: n_eq = C.shape[0] else: n_eq = 0 # reshape inputs if len(f.shape) == 2: f = np.reshape(f, f.shape[0]) # build program prog = MathematicalProgram() x = prog.NewContinuousVariables(n_x) inequalities = [] for i in range(n_ineq): lhs = A[i, :] + 1.e-20 * np.random.rand( (n_x) ) # drake raises a RuntimeError if the in the expression x does not appear (e.g.: 0 x <= 1) rhs = b[i] + 1.e-15 * np.random.rand( 1 ) # in case the constraint is 0 x <= 0 the previous trick ends up adding the constraint x <= 0 to the program... inequalities.append(prog.AddLinearConstraint(lhs.dot(x) <= rhs)) for i in range(n_eq): prog.AddLinearConstraint(C[i, :].dot(x) == d[i]) prog.AddLinearCost(f.dot(x)) # solve solver = GurobiSolver() prog.SetSolverOption(solver.solver_type(), "OutputFlag", 0) result = prog.Solve() # initialize output sol = { 'min': None, 'argmin': None, 'active_set': None, 'multiplier_inequality': None, 'multiplier_equality': None } if result == SolutionResult.kSolutionFound: sol['argmin'] = prog.GetSolution(x).reshape(n_x, 1) sol['min'] = f.dot(sol['argmin'])[0] sol['active_set'] = np.where( A.dot(sol['argmin']) - b > -tol)[0].tolist() # retrieve multipliers through KKT conditions M = A[sol['active_set'], :].T if n_eq > 0: M = np.hstack((M, C.T)) m = np.linalg.pinv(M).dot(-f.reshape(n_x, 1)) sol['multiplier_inequality'] = np.zeros((n_ineq, 1)) for i, j in enumerate(sol['active_set']): sol['multiplier_inequality'][j, 0] = m[i, :] if n_eq > 0: sol['multiplier_equality'] = m[len(sol['active_set']):, :] return sol
for i in range(num_samples - 1): AddDirectCollocationConstraint(dircol_constraint, h[ti], x[ti][:, i], x[ti][:, i + 1], u[ti][:, i], u[ti][:, i + 1], prog) for i in range(num_samples): prog.AddQuadraticCost([1.], [0.], u[ti][:, i]) # prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()])) # prog.AddBoundingBoxConstraint([0.], [0.], u[ti][:,i]) # prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x)) # prog.AddCost(final_cost, x[ti][:,-1]) #prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation) prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out") result = prog.Solve() print(result) print(prog.GetSolution(K)) print(prog.GetSolution(K).dot(control_basis(x[0][:, 0]))) #if (result != SolutionResult.kSolutionFound): # f = open('/tmp/snopt.out', 'r') # print(f.read()) # f.close() #for ti in range(num_trajectories): # h_sol = prog.GetSolution(h[ti])[0] # breaks = [h_sol*i for i in range(num_samples)] # knots = prog.GetSolution(x[ti]) # x_trajectory = PiecewisePolynomial.Cubic(breaks, knots, False)
class HybridModelPredictiveController(object): def __init__(self, S, N, Q, R, P, X_N): # store inputs self.S = S self.N = N self.Q = Q self.R = R self.P = P self.X_N = X_N # mpMIQP self.build_mpmiqp() def build_mpmiqp(self): # express the constrained dynamics as a list of polytopes in the (x,u,x+)-space P = graph_representation(self.S) m = big_m(P) # initialize program self.prog = MathematicalProgram() self.x = [] self.u = [] self.d = [] obj = 0. self.binaries_lower_bound = [] # initial conditions (set arbitrarily to zero in the building phase) self.x.append(self.prog.NewContinuousVariables(self.S.nx)) self.initial_condition = [] for k in range(self.S.nx): self.initial_condition.append(self.prog.AddLinearConstraint(self.x[0][k] == 0.).evaluator()) # loop over time for t in range(self.N): # create input, mode and next state variables self.u.append(self.prog.NewContinuousVariables(self.S.nu)) self.d.append(self.prog.NewBinaryVariables(self.S.nm)) self.x.append(self.prog.NewContinuousVariables(self.S.nx)) # enforce constrained dynamics (big-m methods) xux = np.concatenate((self.x[t], self.u[t], self.x[t+1])) for i in range(self.S.nm): mi_sum = np.sum([m[i][j] * self.d[t][j] for j in range(self.S.nm) if j != i], axis=0) for k in range(P[i].A.shape[0]): self.prog.AddLinearConstraint(P[i].A[k].dot(xux) <= P[i].b[k] + mi_sum[k]) # SOS1 on the binaries self.prog.AddLinearConstraint(sum(self.d[t]) == 1.) # stage cost to the objective obj += .5 * self.u[t].dot(self.R).dot(self.u[t]) obj += .5 * self.x[t].dot(self.Q).dot(self.x[t]) # terminal constraint for k in range(self.X_N.A.shape[0]): self.prog.AddLinearConstraint(self.X_N.A[k].dot(self.x[self.N]) <= self.X_N.b[k]) # terminal cost obj += .5 * self.x[self.N].dot(self.P).dot(self.x[self.N]) self.objective = self.prog.AddQuadraticCost(obj) # set solver self.solver = GurobiSolver() self.prog.SetSolverOption(self.solver.solver_type(), 'OutputFlag', 1) def set_initial_condition(self, x0): for k, c in enumerate(self.initial_condition): c.UpdateLowerBound(x0[k:k+1]) c.UpdateUpperBound(x0[k:k+1]) def feedforward(self, x0): # overwrite initial condition self.set_initial_condition(x0) # solve MIQP result = self.solver.Solve(self.prog) # check feasibility if result != SolutionResult.kSolutionFound: return None, None, None, None # get cost obj = self.prog.EvalBindingAtSolution(self.objective)[0] # store argmin in list of vectors u = [self.prog.GetSolution(ut) for ut in self.u] x = [self.prog.GetSolution(xt) for xt in self.x] d = [self.prog.GetSolution(dt) for dt in self.d] # retrieve mode sequence and check integer feasibility ms = [np.argmax(dt) for dt in d] return u, x, ms, obj def feedback(self, x0): # get feedforward and extract first input u_feedforward = self.feedforward(x0)[0] if u_feedforward is None: return None return u_feedforward[0]
def make_multiple_dircol_trajectories(num_trajectories, num_samples, initial_conditions=None): from pydrake.all import ( AutoDiffXd, Expression, Variable, MathematicalProgram, SolverType, SolutionResult, DirectCollocationConstraint, AddDirectCollocationConstraint, PiecewisePolynomial, ) import pydrake.symbolic as sym from pydrake.examples.pendulum import (PendulumPlant) # initial_conditions maps (ti) -> [1xnum_states] initial state if initial_conditions is not None: initial_conditions = intial_cond_dict[initial_conditions] assert callable(initial_conditions) plant = PendulumPlant() context = plant.CreateDefaultContext() dircol_constraint = DirectCollocationConstraint(plant, context) # num_trajectories = 15; # num_samples = 15; prog = MathematicalProgram() # K = prog.NewContinuousVariables(1,7,'K') def cos(x): if isinstance(x, AutoDiffXd): return x.cos() elif isinstance(x, Variable): return sym.cos(x) return math.cos(x) def sin(x): if isinstance(x, AutoDiffXd): return x.sin() elif isinstance(x, Variable): return sym.sin(x) return math.sin(x) def final_cost(x): return 100. * (cos(.5 * x[0])**2 + x[1]**2) h = [] u = [] x = [] xf = (math.pi, 0.) for ti in range(num_trajectories): h.append(prog.NewContinuousVariables(1)) prog.AddBoundingBoxConstraint(.01, .2, h[ti]) # prog.AddQuadraticCost([1.], [0.], h[ti]) # Added by me, penalize long timesteps u.append(prog.NewContinuousVariables(1, num_samples, 'u' + str(ti))) x.append(prog.NewContinuousVariables(2, num_samples, 'x' + str(ti))) # Use Russ's initial conditions, unless I pass in a function myself. if initial_conditions is None: x0 = (.8 + math.pi - .4 * ti, 0.0) else: x0 = initial_conditions(ti) assert len(x0) == 2 #TODO: undo this hardcoding. prog.AddBoundingBoxConstraint(x0, x0, x[ti][:, 0]) nudge = np.array([.2, .2]) prog.AddBoundingBoxConstraint(xf - nudge, xf + nudge, x[ti][:, -1]) # prog.AddBoundingBoxConstraint(xf, xf, x[ti][:,-1]) for i in range(num_samples - 1): AddDirectCollocationConstraint(dircol_constraint, h[ti], x[ti][:, i], x[ti][:, i + 1], u[ti][:, i], u[ti][:, i + 1], prog) for i in range(num_samples): prog.AddQuadraticCost([1.], [0.], u[ti][:, i]) # prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()])) # prog.AddBoundingBoxConstraint([-3.], [3.], u[ti][:,i]) # prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x)) # prog.AddCost(final_cost, x[ti][:,-1]) # prog.AddCost(h[ti][0]*100) # Try to penalize using more time than it needs? #prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation) prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out") # result = prog.Solve() # print(result) # print(prog.GetSolution(K)) # print(prog.GetSolution(K).dot(control_basis(x[0][:,0]))) #if (result != SolutionResult.kSolutionFound): # f = open('/tmp/snopt.out', 'r') # print(f.read()) # f.close() return prog, h, u, x