def SOS_compute_2(self, l_coeff, S, rho_max=10.): prog = MathematicalProgram() # fix V and lbda, searcu for u and rho x = prog.NewIndeterminates(2, "x") # get lbda from before l = np.array([x[1], x[0], 1]) lbda = l.dot(np.dot(l_coeff, l)) # Define u K = prog.NewContinuousVariables(2, "K") # Fixed Lyapunov V = x.dot(np.dot(S, x)) Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0] # rho is decision variable now rho = prog.NewContinuousVariables(1, "rho")[0] prog.AddSosConstraint(-Vdot - lbda * (rho - V)) prog.AddLinearConstraint(rho <= rho_max) prog.AddLinearCost(-rho) prog.Solve() rho = prog.GetSolution(rho) K = prog.GetSolution(K) return rho, K
def SOS_compute_3(self, K, l_coeff, rho_max=10.): prog = MathematicalProgram() # fix u and lbda, search for V and rho x = prog.NewIndeterminates(2, "x") # get lbda from before l = np.array([x[1], x[0], 1]) lbda = l.dot(np.dot(l_coeff, l)) # rho is decision variable now rho = prog.NewContinuousVariables(1, "rho")[0] # create lyap V s = prog.NewContinuousVariables(4, "s") S = np.array([[s[0], s[1]], [s[2], s[3]]]) V = x.dot(np.dot(S, x)) Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0] prog.AddSosConstraint(V) prog.AddSosConstraint(-Vdot - lbda * (rho - V)) prog.AddLinearCost(-rho) prog.AddLinearConstraint(rho <= rho_max) prog.Solve() rho = prog.GetSolution(rho) s = prog.GetSolution(s) return s, rho
def intercepting_with_obs_avoidance(self, p0, v0, pf, vf, T, obstacles=None, p_puck=None): """Intercepting trajectory that avoids obs""" x0 = np.array(np.concatenate((p0, v0), axis=0)) xf = np.concatenate((pf, vf), axis=0) prog = MathematicalProgram() # state and control inputs N = int(T / self.params.dt) # number of time steps state = prog.NewContinuousVariables(N + 1, 4, 'state') cmd = prog.NewContinuousVariables(N, 2, 'input') # Initial and final state prog.AddLinearConstraint(eq(state[0], x0)) #prog.AddLinearConstraint(eq(state[-1], xf)) prog.AddQuadraticErrorCost(Q=10.0 * np.eye(4), x_desired=xf, vars=state[-1]) self.add_dynamics(prog, N, state, cmd) ## Input saturation self.add_input_limits(prog, cmd, N) # Arena constraints self.add_arena_limits(prog, state, N) for k in range(N): prog.AddQuadraticCost(cmd[k].flatten().dot(cmd[k])) # Add non-linear constraints - will solve with SNOPT # Avoid other players if obstacles != None: for obs in obstacles: self.avoid_other_player_nl(prog, state, obs, N) # avoid hitting the puck while generating a kicking trajectory if not p_puck.any(None): self.avoid_puck_nl(prog, state, N, p_puck) solver = SnoptSolver() result = solver.Solve(prog) solution_found = result.is_success() if not solution_found: print("Solution not found for intercepting_with_obs_avoidance") u_values = result.GetSolution(cmd) return solution_found, u_values.T
def optimization(self, quad_plants, ball_plants, contexts): prog = MathematicalProgram() #Time steps likely requires a line search. #Find the number of time steps until a ball reaches a threshold so that a quadrotor can catch it. N = 200 #Number of time steps n_u = 2*len(plants) n_x = 6*len(quad_plants) + 4*len(ball_plants) n_quads = len(quad_plants) n_balls = len(ball_plants) u = np.empty((self.n_u, N-1), dtype=Variable) x = np.empty((self.n_x, N), dtype=Variable) I = 0.00383 r = 0.25 mass = 0.486 g = 9.81 #Add all the decision variables for n in range(N-1): u[:,n] = prog.NewContinuousVariables(self.n_u, 'u' + str(n)) x[:,n] = prog.NewContinuousVariables(self.n_x, 'x' + str(n)) x[:,N-1] = prog.NewContinuousVariables(self.n_x, 'x' + str(N)) #Start at the correct initial conditions #Connect quadrotor and ball state variables to the initial condition of mathprog x0 = np.empty((self.n_x,1)) for k in range(n_quads + n_balls): if k < n_quads: x0[6*k:6*k+5] = self.quad_plants[k] #??? prog.AddBoundingBoxConstraint(x0, x0, x[:,0]) for n in range(N-1): for k in range(n_quads + n_balls): #Handles no collisions if k < n_quads: #Quad dynamic constraints dynamics_constraint_vel = #? dynamics_constraint_acc = #? prog.AddConstraint(dynamics_constraint_vel[0, 0]) prog.AddConstraint(dynamics_constraint_vel[1, 0]) prog.AddConstraint(dynamics_constraint_vel[2, 0]) prog.AddConstraint(dynamics_constraint_acc[0, 0]) prog.AddConstraint(dynamics_constraint_acc[1, 0]) prog.AddConstraint(dynamics_constraint_acc[2, 0]) else: #Ball dynamic constraints dynamics_constraint_vel = # dynamics_constraint_acc = # prog.AddConstraint(dynamics_constraint_vel[0, 0]) prog.AddConstraint(dynamics_constraint_vel[1, 0]) prog.AddConstraint(dynamics_constraint_acc[0, 0]) prog.AddConstraint(dynamics_constraint_acc[1, 0])
def initialize_problem(n_x, n_u, N, x0): ''' returns base MathematicalProgram object and empty decision vars ''' prog = MathematicalProgram() # Decision Variables x = prog.NewContinuousVariables(n_x, N, 'x') u = prog.NewContinuousVariables(n_u, N-1, 'u') z = [] # placeholder for obstacle binary variables q = [] # placeholder for obstacle slack buffer variables # initial conditions constraints prog.AddBoundingBoxConstraint(x0, x0, x[:, 0]) decision_vars = [x, u, z, q] return prog, decision_vars
def CheckLevelSet(self, prev_x, x0, Vs, Vsdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = Vs.Substitute(dict(zip(prev_x, x))) Vdot = Vsdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1,'a')[0] #mapping = dict(zip(x, np.ones(len(x)))) #V_norm = 0.0*V #for i in range(len(x)): # basis = np.ones(len(x)) # V_norm = V_norm + V.Substitute(dict(zip(x, basis))) #V = V/V_norm #Vdot = Vdot/V_norm #prog.AddConstraint(V_norm == 0) # in relative state (lambda(xbar) Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar) prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) #import pdb; pdb.set_trace() result = Solve(prog) if(not result.is_success()): print('%s, %s' %(result.get_solver_id().name(),result.get_solution_result()) ) print('slack = %f' %(result.GetSolution(slack)) ) print('Rho = %f' %(rho)) #assert result.is_success() return -1.0 return result.GetSolution(slack)
def SOS_compute_1(self, S, rho_prev): # fix V and rho, search for L and u prog = MathematicalProgram() x = prog.NewIndeterminates(2, "x") # Define u K = prog.NewContinuousVariables(2, "K") # Fixed Lyapunov V = x.dot(np.dot(S, x)) Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0] # Define the Lagrange multipliers. (lambda_, constraint) = prog.NewSosPolynomial(Variables(x), 2) prog.AddLinearConstraint(K[0] * x[0] <= 2.5) prog.AddSosConstraint(-Vdot - lambda_.ToExpression() * (rho_prev - V)) result = prog.Solve() # print(lambda_.ToExpression()) # print(lambda_.decision_variables()) lc = [prog.GetSolution(var) for var in lambda_.decision_variables()] lbda_coeff = np.ones([3, 3]) lbda_coeff[0, 0] = lc[0] lbda_coeff[0, 1] = lbda_coeff[1, 0] = lc[1] lbda_coeff[2, 0] = lbda_coeff[0, 2] = lc[2] lbda_coeff[1, 1] = lc[3] lbda_coeff[2, 1] = lbda_coeff[1, 2] = lc[4] lbda_coeff[2, 2] = lc[5] return lbda_coeff
def run_simple_mathematical_program(): print "\n\nsimple mathematical program" mp = MathematicalProgram() x = mp.NewContinuousVariables(1, "x") mp.AddLinearCost(x[0] * 1.0) mp.AddLinearConstraint(x[0] >= 1) print mp.Solve() print mp.GetSolution(x) print "(finished) simple mathematical program"
def solve_lcp(P, q): prog = MathematicalProgram() x = prog.NewContinuousVariables(q.size) prog.AddLinearComplementarityConstraint(P, q, x) result = Solve(prog) status = result.is_success() z = result.GetSolution(x) return (z, status)
def run_complex_mathematical_program(): print "\n\ncomplex mathematical program" mp = MathematicalProgram() x = mp.NewContinuousVariables(1, 'x') mp.AddCost(cost, x) mp.AddConstraint(quad_constraint, [1.0], [2.0], x) mp.SetInitialGuess(x, [1.1]) print mp.Solve() res = mp.GetSolution(x) print res print "(finished) complex mathematical program"
def test_create_constraint_input_array(self): prog = MathematicalProgram() q = prog.NewContinuousVariables(1, 'q') r = prog.NewContinuousVariables(rows=2, cols=3, name='r') q_val = 0.5 r_val = np.arange(6).reshape((2,3)) ''' array([[0, 1, 2], [3, 4, 5]]) ''' constraint = prog.AddConstraint(le(q, r[0,0] + 2*r[1,1])) input_array = create_constraint_input_array(constraint, { "q": q_val, "r": r_val }) expected_input_array = [0.5, 0, 4] np.testing.assert_array_almost_equal(input_array, expected_input_array) z = prog.NewContinuousVariables(2, 'z') z_val = np.array([0.0, 0.5]) # https://stackoverflow.com/questions/64736910/using-le-or-ge-with-scalar-left-hand-side-creates-unsized-formula-array # constraint = prog.AddConstraint(le(z[1], r[0,2] + 2*r[1,0])) constraint = prog.AddConstraint(le([z[1]], r[0,2] + 2*r[1,0])) input_array = create_constraint_input_array(constraint, { "z": z_val, "r": r_val }) expected_input_array = [3, 2, 0.5] np.testing.assert_array_almost_equal(input_array, expected_input_array) a = prog.NewContinuousVariables(rows=2, cols=1, name='a') a_val = np.array([[1.0], [2.0]]) constraint = prog.AddConstraint(eq(a[0], a[1])) input_array = create_constraint_input_array(constraint, { "a": a_val }) expected_input_array = [1.0, 2.0] np.testing.assert_array_almost_equal(input_array, expected_input_array)
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1)[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) assert result.is_success() return result.GetSolution(slack)
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): #import pdb; pdb.set_trace() prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1,'a')[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() #print('V degree: %d' %(Polynomial(V).TotalDegree())) #print('Vdot degree: %d' %(Polynomial(Vdot).TotalDegree())) #print('SOS degree: %d' %(Polynomial(-Vdot + Lambda*(V - rho) - slack*V).TotalDegree())) prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) #assert result.is_success() return result.GetSolution(slack)
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
Mout = solve_continuous_are(Aout, Bout, Q, R) # print("Mout",Mout) # print("M_out:", Mout) # Minimum and maximum eigenvalues for Q and Mout used for CLF-QP constraint 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)
# Lyapunov function V = x.dot(P).dot(x) V_dot = 2*x.dot(P).dot(f(x)) # degree of the polynomial lambda(x) # no need to change it, but if you really want to, # keep l_deg even and do not set l_deg greater than 10 l_deg = 4 assert l_deg % 2 == 0 # SOS Lagrange multipliers l = prog2.NewSosPolynomial(Variables(x), l_deg)[0].ToExpression() # level set as optimization variable rho = prog2.NewContinuousVariables(1, 'rho')[0] # write here the SOS condition described in the "Not quite there yet..." section above prog2.AddSosConstraint(x.dot(x)*(V-rho) - l*V_dot) # insert here the objective function (maximize rho) prog2.AddLinearCost(-rho) # solve program only if the lines above are filled if len(prog2.GetAllConstraints()) != 0: # solve SOS program result = Solve(prog2) # get maximum rho assert result.is_success()
def ComputeControlInput(self, x, t): # Set up things you might want... q = x[0:self.nq] v = x[self.nq:] kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()]) M = self.hand.massMatrix(kinsol) C = self.hand.dynamicsBiasTerm(kinsol, {}, None) B = self.hand.B # Assume grasp points are achieved, and calculate # contact jacobian at those points, as well as the current # grasp points and normals (in WORLD FRAME). grasp_points_world_now = self.transform_grasp_points_manipuland(q) grasp_normals_world_now = self.transform_grasp_normals_manipuland(q) J_manipuland = jacobian(self.transform_grasp_points_manipuland, q) ee_points_now = self.transform_grasp_points_fingers(q) J_manipulator = jacobian(self.transform_grasp_points_fingers, q) # The contact jacobian (Phi), for contact forces coming from # point contacts, describes how the movement of the contact point # maps into the joint coordinates of the robot. We can get this # by combining the jacobians derived from forward kinematics to each # of the two bodies that are in contact, evaluated at the contact point # in each body's frame. J_contact = J_manipuland - J_manipulator # Cut off the 3rd dimension, which should always be 0 J_contact = J_contact[0:2, :, :] # Given these, the manipulator equations enter as a linear constraint # M qdd + C = Bu + J_contact lambda # (the combined effects of lambda on the robot). # The list of grasp points, in manipuland frame, that we'll use. n_cf = len(self.grasp_points) # The evaluated desired manipuland posture. manipuland_qdes = self.GetDesiredObjectPosition(t) # The desired robot (arm) posture, calculated via inverse kinematics # to achieve the desired grasp points on the object in its current # target posture. qdes, info = self.ComputeTargetPosture(x, manipuland_qdes) if info != 1: if not self.shut_up: print "Warning: target posture IK solve got info %d " \ "when computing goal posture at least once during " \ "simulation. This means the grasp points was hard to " \ "achieve given the current object posture. This is " \ "occasionally OK, but indicates that your controller " \ "is probably struggling a little." % info self.shut_up = True # From here, it's up to you. Following the guidelines in the problem # set, implement a controller to achieve the specified goals. mp = MathematicalProgram() #control variables = u = self.nu x 1 #ddot{q} as variable w/ q's shape controls = mp.NewContinuousVariables(self.nu, "controls") qdd = mp.NewContinuousVariables(q.shape[0], "qdd") #make variables for lambda's and beta's k = 0 b = mp.NewContinuousVariables(2, "betas_%d" % k) betas = b for i in range(len(self.grasp_points)): b = mp.NewContinuousVariables( 2, "betas_%d" % k) #pair of betas for each contact point betas = np.vstack((betas, b)) mp.AddLinearConstraint( betas[i, 0] >= 0).evaluator().set_description( "beta_0 greater than 0 for %d" % i) mp.AddLinearConstraint( betas[i, 1] >= 0).evaluator().set_description( "beta_1 greater than 0 for %d" % i) #c_0=normals+mu*tangent #c1 = normals-mu*tangent n = grasp_normals_world_now.T[:, 0:2] #row=contact point? t = get_perpendicular2d(n[0]) c_i1 = n[0] + np.dot(self.mu, t) c_i2 = n[0] - np.dot(self.mu, t) l = c_i1.dot(betas[0, 0]) + c_i2.dot(betas[0, 1]) lambdas = l for i in range(1, len(self.grasp_points)): n = grasp_normals_world_now.T[:, 0: 2] #row=contact point? transpose then index t = get_perpendicular2d(n[i]) c_i1 = n[i] - np.dot(self.mu, t) c_i2 = n[i] + np.dot(self.mu, t) l = c_i1.dot(betas[i, 0]) + c_i2.dot(betas[i, 1]) lambdas = np.vstack((lambdas, l)) control_period = 0.0333 #Constrain the dyanmics dyn = M.dot(qdd) + C - B.dot(controls) for i in range(q.shape[0]): j_c = 0 for l in range(len(self.grasp_points)): j_sub = J_contact[:, l, :].T j_c += j_sub.dot(lambdas[l]) # print(j_c.shape) # print(dyn.shape) mp.AddConstraint(dyn[i] - j_c[i] == 0) #PD controller using kinematics Kp = 100.0 Kd = 1.0 control_period = 0.0333 next_q = q + v.dot(control_period) + np.dot(qdd.dot(control_period**2), .5) next_q_dot = v + qdd.dot(control_period) mp.AddQuadraticCost(Kp * (qdes - next_q).dot((qdes - next_q).T) + Kd * (next_q_dot).dot(next_q_dot.T)) Kp_ext = 100. mp.AddQuadraticCost(Kp_ext * (qdes - next_q)[-3:].dot( (qdes - next_q)[-3:].T)) res = Solve(mp) c = res.GetSolution(controls) return c
def PlanGraspPoints(self): # First, extract the bounding geometry of the object. # Generally, our geometry is coming from 3d models, so # we have to do some legwork to extract 2D geometry. For # the examples we'll use in this set, we'll assume # that extracting the convex hull of the first visual element # is a good representation of the object geometry. (This is # not great! But it'll do the job for us, since we're going # to experiment with only simple objects.) kinsol = self.hand.doKinematics( self.x_nom[0:self.hand.get_num_positions()]) self.manipuland_link_index = \ self.hand.FindBody(self.manipuland_link_name).get_body_index() body = self.hand.get_body(self.manipuland_link_index) # For projecting into XY plane Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]]) all_points = ExtractPlanarObjectGeometryConvexHull(body, Tview) # For later use: precompute the fingertip positions of the # robot in the nominal posture. nominal_fingertip_points = np.empty((2, self.num_fingers)) for i in range(self.num_fingers): nominal_fingertip_points[:, i] = self.hand.transformPoints( kinsol, self.fingertip_position, self.finger_link_indices[i], 0)[0:2, 0] # Search for an optimal grasp with N points random.seed(42) np.random.seed(42) def get_random_point_and_normal_on_surface(all_points): num_points = all_points.shape[1] i = random.choice(range(num_points - 1)) first_point = np.asarray([all_points[0][i], all_points[1][i]]) second_point = np.asarray( [all_points[0][i + 1], all_points[1][i + 1]]) first_to_second = second_point - first_point # Clip to avoid interpolating close to a corner interpolate_param = np.clip(np.random.rand(), 0.2, 0.8) rand_point = first_point + interpolate_param * first_to_second normal = np.array([-first_to_second[1], first_to_second[0]]) \ / np.linalg.norm(first_to_second) return rand_point, normal best_conv_volume = 0 best_points = [] best_normals = [] best_finger_assignments = [] for i in range(self.n_grasp_search_iters): grasp_points = [] normals = [] for j in range(self.num_fingers): point, normal = \ get_random_point_and_normal_on_surface(all_points) # check for duplicates or close points -- fingertip # radius is 0.2, so require twice that margin to avoid # intersection fingertips. num_rejected = 0 while min([1.0] + [ np.linalg.norm(grasp_point - point, 2) for grasp_point in grasp_points ]) <= 0.4: point, normal = \ get_random_point_and_normal_on_surface(all_points) num_rejected += 1 if num_rejected > 10000: print "Rejected 10000 points in a row due to crowding." \ " Your object is a bit too small for your number of" \ " fingers." break grasp_points.append(point) normals.append(normal) if achieves_force_closure(grasp_points, normals, self.mu): # Test #1: Rank in terms of convex hull volume of grasp points volume = compute_convex_hull_volume(grasp_points) if volume < best_conv_volume: continue # Test #2: Does IK work for this point? self.grasp_points = grasp_points self.grasp_normals = normals # Pick optimal finger assignment that # minimizes distance between fingertips in the # nominal posture, and the chosen grasp points grasp_points_world = self.transform_grasp_points_manipuland( self.x_nom)[0:2, :] prog = MathematicalProgram() # We'd *really* like these to be binary variables, # but unfortunately don't have a MIP solver available in the # course docker container. Instead, we'll solve an LP, # and round the result to the nearest feasible integer # solutions. Intuitively, this LP should probably reach its # optimal value at an extreme point (where the variables # all take integer value). I've not yet observed this # not occuring in practice! assignment_vars = prog.NewContinuousVariables( self.num_fingers, self.num_fingers, "assignment") for grasp_i in range(self.num_fingers): # Every row and column of assignment vars sum to one -- # each finger matches to one grasp position prog.AddLinearConstraint( np.sum(assignment_vars[:, grasp_i]) == 1.) prog.AddLinearConstraint( np.sum(assignment_vars[grasp_i, :]) == 1.) for finger_i in range(self.num_fingers): # If this grasp assignment is active, # add a cost equal to the distance between # nominal pose and grasp position prog.AddLinearCost( assignment_vars[grasp_i, finger_i] * np.linalg.norm(grasp_points_world[:, grasp_i] - nominal_fingertip_points[:, finger_i])) prog.AddBoundingBoxConstraint( 0., 1., assignment_vars[grasp_i, finger_i]) result = Solve(prog) assignments = result.GetSolution(assignment_vars) # Round assignments to nearest feasible set claimed = [False] * self.num_fingers for grasp_i in range(self.num_fingers): order = np.argsort(assignments[grasp_i, :]) fill_i = self.num_fingers - 1 while claimed[order[fill_i]] is not False: fill_i -= 1 if fill_i < 0: raise Exception("Finger association failed. " "Horrible bug. Tell Greg") assignments[grasp_i, :] *= 0. assignments[grasp_i, order[fill_i]] = 1. claimed[order[fill_i]] = True # Populate actual assignments self.grasp_finger_assignments = [] for grasp_i in range(self.num_fingers): for finger_i in range(self.num_fingers): if assignments[grasp_i, finger_i] == 1.: self.grasp_finger_assignments.append( (finger_i, np.array(self.fingertip_position))) continue qinit, info = self.ComputeTargetPosture( self.x_nom, self.x_nom[(self.nq - 3):self.nq], backoff_distance=0.2) if info != 1: continue best_conv_volume = volume best_points = grasp_points best_normals = normals best_finger_assignments = self.grasp_finger_assignments if len(best_points) == 0: print "After %d attempts, couldn't find a good grasp "\ "for this object." % self.n_grasp_search_iters print "Proceeding with a horrible random guess." best_points = [ np.random.uniform(-1., 1., 2) for i in range(self.num_fingers) ] best_normals = [ np.random.uniform(-1., 1., 2) for i in range(self.num_fingers) ] best_finger_assignments = [(i, self.fingertip_position) for i in range(self.num_fingers)] self.grasp_points = best_points self.grasp_normals = best_normals self.grasp_finger_assignments = best_finger_assignments
def apply_angular_velocity_to_quaternion(q, w_axis, w_mag, t): delta_q = np.hstack( [np.cos(w_mag * t / 2.0), w_axis * np.sin(w_mag * t / 2.0)]) return quat_multiply(q, delta_q) def backward_euler(q_qprev_v_dt): q, qprev, w_axis, w_mag, dt = np.split( q_qprev_v_dt, [4, 4 + 4, 4 + 4 + 3, 4 + 4 + 3 + 1]) return q - apply_angular_velocity_to_quaternion(qprev, w_axis, w_mag, dt) N = 4 prog = MathematicalProgram() q = prog.NewContinuousVariables(rows=N, cols=4, name='q') w_axis = prog.NewContinuousVariables(rows=N, cols=3, name="w_axis") w_mag = prog.NewContinuousVariables(rows=N, cols=1, name="w_mag") # dt = [0.0] + [1.0/(N-1)] * (N-1) dt = prog.NewContinuousVariables(rows=N, cols=1, name="dt") for k in range(N): (prog.AddConstraint(lambda x: [x @ x], [1], [ 1 ], q[k]).evaluator().set_description(f"q[{k}] unit quaternion constraint")) # Impose unit length constraint on the rotation axis. (prog.AddConstraint(lambda x: [x @ x], [1], [1], w_axis[k]).evaluator().set_description( f"w_axis[{k}] unit axis constraint")) for k in range(1, N): (prog.AddConstraint(
def ComputeControlInput(self, x, t): # Set up things you might want... q = x[0:self.nq] v = x[self.nq:] kinsol = self.hand.doKinematics(x) M = self.hand.massMatrix(kinsol) C = self.hand.dynamicsBiasTerm(kinsol, {}, None) B = self.hand.B # Assume grasp points are achieved, and calculate # contact jacobian at those points, as well as the current # grasp points and normals (in WORLD FRAME). grasp_points_world_now = self.transform_grasp_points_manipuland(q) grasp_normals_world_now = self.transform_grasp_normals_manipuland(q) J_manipuland = jacobian( self.transform_grasp_points_manipuland, q) ee_points_now = self.transform_grasp_points_fingers(q) J_manipulator = jacobian( self.transform_grasp_points_fingers, q) # The contact jacobian (Phi), for contact forces coming from # point contacts, describes how the movement of the contact point # maps into the joint coordinates of the robot. We can get this # by combining the jacobians derived from forward kinematics to each # of the two bodies that are in contact, evaluated at the contact point # in each body's frame. J_contact = J_manipuland - J_manipulator # Cut off the 3rd dimension, which should always be 0 J_contact = J_contact[0:2, :, :] normals = grasp_normals_world_now[0:2, :].T # Given these, the manipulator equations enter as a linear constraint # M qdd + C = Bu + J_contact lambda # (the combined effects of lambda on the robot). # The list of grasp points, in manipuland frame, that we'll use. n_cf = len(self.grasp_points) # The evaluated desired manipuland posture. manipuland_qdes = self.GetDesiredObjectPosition(t) # The desired robot (arm) posture, calculated via inverse kinematics # to achieve the desired grasp points on the object in its current # target posture. qdes, info = self.ComputeTargetPosture(x, manipuland_qdes) if info != 1: if not self.shut_up: print "Warning: target posture IK solve got info %d " \ "when computing goal posture at least once during " \ "simulation. This means the grasp points was hard to " \ "achieve given the current object posture. This is " \ "occasionally OK, but indicates that your controller " \ "is probably struggling a little." % info self.shut_up = True # From here, it's up to you. Following the guidelines in the problem # set, implement a controller to achieve the specified goals. ''' YOUR CODE HERE ''' # This is not proper orthogonal vector...but it somehow makes it work... # whereas the correct does not def ortho2(x): return np.array([x[0],-x[1]]) #return np.array([x[1],-x[0]]) prog = MathematicalProgram() dt = self.control_period u = prog.NewContinuousVariables(self.nu, "u") qdd = prog.NewContinuousVariables(q.shape[0], "qdd") for i in range(qdd.shape[0]): prog.AddLinearConstraint(qdd[i] <= 40) prog.AddLinearConstraint(qdd[i] >= -40) #prog.AddLinearConstraint(v[i] + qdd[i] * dt <= 80) #prog.AddLinearConstraint(v[i] - qdd[i] * dt >= -80) beta = prog.NewContinuousVariables(n_cf, 2, "beta") #prog.AddQuadraticCost(0.1*np.sum(beta**2)) for i in range(n_cf): prog.AddLinearConstraint(beta[i,0] >= 0) prog.AddLinearConstraint(beta[i,1] >= 0) prog.AddLinearConstraint(beta[i,0] <= 10) prog.AddLinearConstraint(beta[i,1] <= 10) c = np.zeros((n_cf,2,2)) for i in range(n_cf): c[i,0] = normals[i] - self.mu * ortho2(normals[i]) c[i,1] = normals[i] + self.mu * ortho2(normals[i]) const = M.dot(qdd) + C - B.dot(u) ## eq 0 for i in range(n_cf): lambda_i = c[i,0]*beta[i,0] + c[i,1]*beta[i,1] tmp = J_contact[:, i, :].T.dot(lambda_i) const -= tmp for i in range(const.shape[0]): prog.AddLinearConstraint(const[i] == 0.0) Kp = 1000 Kd = 10 #v2 = v + dt * qdd #q2 = q + v * dt + 0.5 * qdd * dt*dt v2 = v + dt * qdd q2 = q + (v+v2)/2 * dt #+ 0.5 * qdd * dt*dt #v2 = v + dt * qdd #q2 = q + v2 * dt prog.AddQuadraticCost(Kp * np.sum((qdes - q2) ** 2) + Kd * np.sum(v2**2)) result = prog.Solve() u = prog.GetSolution(u) return u
def compute_trajectory(self, obst_idx, x_out, y_out, ux_out, uy_out, pose_initial=[0., 0., 0., 0.], dt=0.05): ''' Find trajectory with MILP input u are tyhe velocities (xd, yd) dt 0.05 according to a rate of 20 Hz ''' mp = MathematicalProgram() N = 30 k = 0 # define input trajectory and state traj u = mp.NewContinuousVariables(2, "u_%d" % k) # xd yd input_trajectory = u st = mp.NewContinuousVariables(4, "state_%d" % k) # # binary variables for obstalces constraint c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = c states = st for k in range(1, N): u = mp.NewContinuousVariables(2, "u_%d" % k) input_trajectory = np.vstack((input_trajectory, u)) st = mp.NewContinuousVariables(4, "state_%d" % k) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) st = mp.NewContinuousVariables(4, "state_%d" % (N + 1)) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) ### define cost mp.AddLinearCost(100 * (-states[-1, 0])) # go as far forward as possible # mp.AddQuadraticCost(states[-1,1]*states[-1,1]) # time constraint M = 1000 # slack var for obst costraint # state constraint for i in range(2): # initial state constraint x y yaw mp.AddLinearConstraint(states[0, i] <= pose_initial[i]) mp.AddLinearConstraint(states[0, i] >= pose_initial[i]) for i in range(2): # initial state constraint xd yd yawd mp.AddLinearConstraint(states[0, i] <= pose_initial[2 + i] + 1) mp.AddLinearConstraint(states[0, i] >= pose_initial[2 + i] - 1) for i in range(N): # state update according to dynamics state_next = self.quad_dynamics(states[i, :], input_trajectory[i, :], dt) for j in range(4): mp.AddLinearConstraint(states[i + 1, j] <= state_next[j]) mp.AddLinearConstraint(states[i + 1, j] >= state_next[j]) # obstacle constraint for j in range(self.ang_discret - 1): mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3) ang_min = self.angles[j] # lower angle bound of obstacle ang_max = self.angles[j + 1] # higher angle bound of obstaclee if int(obst_idx[j]) < (self.rng_discret - 1): # less than max range measured rng_min = self.ranges[int( obst_idx[j])] # where the obst is at at this angle rng_max = self.ranges[int(obst_idx[j] + 1)] mp.AddLinearConstraint( states[i, 0] <= rng_min - 0.05 + M * obs[i, 4 * j]) # xi <= xobs,low + M*c mp.AddLinearConstraint( states[i, 0] >= rng_max + 0.005 - M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c mp.AddLinearConstraint( states[i, 1] <= states[i, 0] * np.tan(ang_min) - 0.05 + M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c mp.AddLinearConstraint( states[i, 1] >= states[i, 0] * np.tan(ang_max) + 0.05 - M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c # environmnt constraint, dont leave fov mp.AddLinearConstraint( states[i, 1] >= states[i, 0] * np.tan(-self.theta)) mp.AddLinearConstraint( states[i, 1] <= states[i, 0] * np.tan(self.theta)) # bound the inputs # mp.AddConstraint(input_trajectory[i,:].dot(input_trajectory[i,:]) <= 2.5*2.5) # dosnt work with multi int mp.AddLinearConstraint(input_trajectory[i, 0] <= 2.5) mp.AddLinearConstraint(input_trajectory[i, 0] >= -2.5) mp.AddLinearConstraint(input_trajectory[i, 1] <= 0.5) mp.AddLinearConstraint(input_trajectory[i, 1] >= -0.5) mp.Solve() input_trajectory = mp.GetSolution(input_trajectory) trajectory = mp.GetSolution(states) x_out[:] = trajectory[:, 0] y_out[:] = trajectory[:, 1] ux_out[:] = input_trajectory[:, 0] uy_out[:] = input_trajectory[:, 1] return trajectory, input_trajectory
def static_controller(self, qref, verbose=False): """ Generates a controller to maintain a static pose Arguments: qref: (N,) numpy array, the static pose to be maintained Return Values: u: (M,) numpy array, actuations to best achieve static pose f: (C,) numpy array, associated normal reaction forces static_controller generates the actuations and reaction forces assuming the velocity and accelerations are zero. Thus, the equation to solve is: N(q) = B*u + J*f where N is a vector of gravitational and conservative generalized forces, B is the actuation selection matrix, and J is the contact-Jacobian transpose. Currently, static_controller only considers the effects of the normal forces. Frictional forces are not yet supported """ #TODO: Solve for friction forces as well # Check inputs if qref.ndim > 1 or qref.shape[0] != self.multibody.num_positions(): raise ValueError( f"Reference position mut be ({self.multibody.num_positions(),},) array" ) # Set the context context = self.multibody.CreateDefaultContext() self.multibody.SetPositions(context, qref) # Get the necessary properties G = self.multibody.CalcGravityGeneralizedForces(context) Jn, _ = self.GetContactJacobians(context) phi = self.GetNormalDistances(context) B = self.multibody.MakeActuationMatrix() #Collect terms A = np.concatenate([B, Jn.transpose()], axis=1) # Create the mathematical program prog = MathematicalProgram() l_var = prog.NewContinuousVariables(self.num_contacts(), name="forces") u_var = prog.NewContinuousVariables(self.multibody.num_actuators(), name="controls") # Ensure dynamics approximately satisfied prog.AddL2NormCost(A=A, b=-G, vars=np.concatenate([u_var, l_var], axis=0)) # Enforce normal complementarity prog.AddBoundingBoxConstraint(np.zeros(l_var.shape), np.full(l_var.shape, np.inf), l_var) prog.AddConstraint(phi.dot(l_var) == 0) # Solve result = Solve(prog) # Check for a solution if result.is_success(): u = result.GetSolution(u_var) f = result.GetSolution(l_var) if verbose: printProgramReport(result, prog) return (u, f) else: print(f"Optimization failed. Returning zeros") if verbose: printProgramReport(result, prog) return (np.zeros(u_var.shape), np.zeros(l_var.shape))
def MILP_compute_traj(self, obst_idx, x_out, y_out, dx, dy, pose_initial=[0., 0.]): ''' Find trajectory with MILP Outputs trajectory (waypoints) and new K for control ''' mp = MathematicalProgram() N = 8 k = 0 # define state traj st = mp.NewContinuousVariables(2, "state_%d" % k) # # binary variables for obstalces constraint c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = c states = st for k in range(1, N): st = mp.NewContinuousVariables(2, "state_%d" % k) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) st = mp.NewContinuousVariables(2, "state_%d" % (N + 1)) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) # variables encoding max x y dist from obstacle x_margin = mp.NewContinuousVariables(1, "x_margin") y_margin = mp.NewContinuousVariables(1, "y_margin") ### define cost for i in range(N): mp.AddLinearCost(-states[i, 0]) # go as far forward as possible mp.AddLinearCost(-states[-1, 0]) mp.AddLinearCost(-x_margin[0]) mp.AddLinearCost(-y_margin[0]) # bound x y margin so it doesn't explode mp.AddLinearConstraint(x_margin[0] <= 3.) mp.AddLinearConstraint(y_margin[0] <= 3.) # x y is non ngative adn at least above robot radius mp.AddLinearConstraint(x_margin[0] >= 0.05) mp.AddLinearConstraint(y_margin[0] >= 0.05) M = 1000 # slack var for integer things # state constraint for i in range(2): # initial state constraint x y mp.AddLinearConstraint(states[0, i] <= pose_initial[i]) mp.AddLinearConstraint(states[0, i] >= pose_initial[i]) for i in range(N): mp.AddQuadraticCost((states[i + 1, 1] - states[i, 1])**2) mp.AddLinearConstraint(states[i + 1, 0] <= states[i, 0] + dx) mp.AddLinearConstraint(states[i + 1, 0] >= states[i, 0] - dx) mp.AddLinearConstraint(states[i + 1, 1] <= states[i, 1] + dy) mp.AddLinearConstraint(states[i + 1, 1] >= states[i, 1] - dy) # obstacle constraint for j in range(self.ang_discret - 1): mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3) ang_min = self.angles[j] # lower angle bound of obstacle ang_max = self.angles[j + 1] # higher angle bound of obstaclee if int(obst_idx[j]) < (self.rng_discret - 1): # less than max range measured rng_min = self.ranges[int( obst_idx[j])] # where the obst is at at this angle rng_max = self.ranges[int(obst_idx[j] + 1)] mp.AddLinearConstraint( states[i, 0] <= rng_min - x_margin[0] + M * obs[i, 4 * j]) # xi <= xobs,low + M*c mp.AddLinearConstraint( states[i, 0] >= rng_max + x_margin[0] - M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c mp.AddLinearConstraint( states[i, 1] <= states[i, 0] * np.tan(ang_min) - y_margin[0] + M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c mp.AddLinearConstraint( states[i, 1] >= states[i, 0] * np.tan(ang_max) + y_margin[0] - M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c # obstacle constraint for last state for j in range(self.ang_discret - 1): mp.AddLinearConstraint(sum(obs[N, 4 * j:4 * j + 4]) <= 3) ang_min = self.angles[j] # lower angle bound of obstacle ang_max = self.angles[j + 1] # higher angle bound of obstaclee if int(obst_idx[j]) < (self.rng_discret - 1): # less than max range measured rng_min = self.ranges[int( obst_idx[j])] # where the obst is at at this angle rng_max = self.ranges[int(obst_idx[j] + 1)] mp.AddLinearConstraint( states[N, 0] <= rng_min - x_margin[0] + M * obs[N, 4 * j]) # xi <= xobs,low + M*c mp.AddLinearConstraint( states[N, 0] >= rng_max + x_margin[0] - M * obs[N, 4 * j + 1]) # xi >= xobs,high - M*c mp.AddLinearConstraint( states[N, 1] <= states[N, 0] * np.tan(ang_min) - y_margin[0] + M * obs[N, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c mp.AddLinearConstraint( states[N, 1] >= states[N, 0] * np.tan(ang_max) + y_margin[0] - M * obs[N, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c mp.Solve() trajectory = mp.GetSolution(states) xm = mp.GetSolution(x_margin) ym = mp.GetSolution(y_margin) x_out[:] = trajectory[:, 0] y_out[:] = trajectory[:, 1] return trajectory, xm[0], ym[0]
def trajopt_simulation(x0, xf, u_max=0.5): # numeric parameters time_interval = .1 time_steps = 400 # initialize optimization prog = MathematicalProgram() # optimization variables state = prog.NewContinuousVariables(time_steps + 1, 3, 'state') u = prog.NewContinuousVariables(time_steps, 1, 'u') # final position constraint # for residual in constraint_state_to_orbit(state[-2], state[-1], xf, 1/u_max, time_interval): # prog.AddConstraint(residual == 0) # initial position constraint prog.AddConstraint(eq(state[0],x0)) # discretized dynamics for t in range(time_steps): residuals = dubins_discrete_dynamics(state[t], state[t+1], u[t], time_interval) for residual in residuals: prog.AddConstraint(residual == 0) # control limits prog.AddConstraint(u[t][0] <= u_max) prog.AddConstraint(-u_max <= u[t][0]) # cost - increases cost if off the orbit p = state[t][:2] - xf[:2] v = (state[t+1][:2] - state[t][:2]) / time_interval # prog.AddCost(time_interval) # prog.AddCost(u[t][0]*u[t][0]*time_interval) # prog.AddCost(p.dot(v)*time_interval) for residual in constraint_state_to_orbit(state[t], state[t+1], xf, 1/u_max, time_interval): # prog.AddConstraint(residual >= 0) prog.AddQuadraticCost(residual) # prog.AddCost((p.dot(p))) # # initial guess state_guess = interpolate_dubins_state( np.array([0, 0, np.pi]), xf, time_steps, time_interval, 1/u_max ) prog.SetInitialGuess(state, state_guess) solver = SnoptSolver() result = solver.Solve(prog) assert result.is_success() # retrieve optimal solution u_opt = result.GetSolution(u).T state_opt = result.GetSolution(state).T return state_opt, u_opt
def intercepting_with_obs_avoidance_bb(self, p0, v0, pf, vf, T, obstacles=None, p_puck=None): """kick while avoiding obstacles using big M formulation and branch and bound""" x0 = np.array(np.concatenate((p0, v0), axis=0)) xf = np.concatenate((pf, vf), axis=0) prog = MathematicalProgram() # state and control inputs N = int(T / self.params.dt) # number of command steps state = prog.NewContinuousVariables(N + 1, 4, 'state') cmd = prog.NewContinuousVariables(N, 2, 'input') # Initial and final state prog.AddLinearConstraint(eq(state[0], x0)) prog.AddLinearConstraint(eq(state[-1], xf)) self.add_dynamics(prog, N, state, cmd) ## Input saturation self.add_input_limits(prog, cmd, N) # Arena constraints self.add_arena_limits(prog, state, N) # Add Mixed-integer constraints - will solve with BB # avoid hitting the puck while generating a kicking trajectory # MILP formulation with B&B solver x_obs_puck = prog.NewBinaryVariables(rows=N + 1, cols=2) # obs x_min, obs x_max y_obs_puck = prog.NewBinaryVariables(rows=N + 1, cols=2) # obs y_min, obs y_max self.avoid_puck_bigm(prog, x_obs_puck, y_obs_puck, state, N, p_puck) # Avoid other players if obstacles != None: x_obs_player = list() y_obs_player = list() for i, obs in enumerate(obstacles): x_obs_player.append(prog.NewBinaryVariables( rows=N + 1, cols=2)) # obs x_min, obs x_max y_obs_player.append(prog.NewBinaryVariables( rows=N + 1, cols=2)) # obs y_min, obs y_max self.avoid_other_player_bigm(prog, x_obs_player[i], y_obs_player[i], state, obs, N) # Solve with simple b&b solver for k in range(N): prog.AddQuadraticCost(cmd[k].flatten().dot(cmd[k])) bb = branch_and_bound.MixedIntegerBranchAndBound( prog, OsqpSolver().solver_id()) result = bb.Solve() if result != result.kSolutionFound: raise ValueError('Infeasible optimization problem.') u_values = np.array([bb.GetSolution(u) for u in cmd]).T solution_found = result.kSolutionFound return solution_found, u_values
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 ComputeControlInput(self, x, t): # Set up things you might want... q = x[0:self.nq] v = x[self.nq:] kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()]) M = self.hand.massMatrix(kinsol) C = self.hand.dynamicsBiasTerm(kinsol, {}, None) B = self.hand.B # Assume grasp points are achieved, and calculate # contact jacobian at those points, as well as the current # grasp points and normals (in WORLD FRAME). grasp_points_world_now = self.transform_grasp_points_manipuland(q) grasp_normals_world_now = self.transform_grasp_normals_manipuland(q) J_manipuland = jacobian( self.transform_grasp_points_manipuland, q) ee_points_now = self.transform_grasp_points_fingers(q) J_manipulator = jacobian( self.transform_grasp_points_fingers, q) # The contact jacobian (Phi), for contact forces coming from # point contacts, describes how the movement of the contact point # maps into the joint coordinates of the robot. We can get this # by combining the jacobians derived from forward kinematics to each # of the two bodies that are in contact, evaluated at the contact point # in each body's frame. J_contact = J_manipuland - J_manipulator # Cut off the 3rd dimension, which should always be 0 J_contact = J_contact[0:2, :, :] # Given these, the manipulator equations enter as a linear constraint # M qdd + C = Bu + J_contact lambda # (the combined effects of lambda on the robot). # The list of grasp points, in manipuland frame, that we'll use. n_cf = len(self.grasp_points) number_of_grasp_points = len(self.grasp_points) # The evaluated desired manipuland posture. manipuland_qdes = self.GetDesiredObjectPosition(t) # The desired robot (arm) posture, calculated via inverse kinematics # to achieve the desired grasp points on the object in its current # target posture. qdes, info = self.ComputeTargetPosture(x, manipuland_qdes) # print('shape' + str(np.shape(qdes))) # print('self.nq' + str(self.nq)) # print('self.nu' + str(self.nu)) if info != 1: if not self.shut_up: print "Warning: target posture IK solve got info %d " \ "when computing goal posture at least once during " \ "simulation. This means the grasp points was hard to " \ "achieve given the current object posture. This is " \ "occasionally OK, but indicates that your controller " \ "is probably struggling a little." % info self.shut_up = True # From here, it's up to you. Following the guidelines in the problem # set, implement a controller to achieve the specified goals. kd = 1 kp = 25 from pydrake.all import MathematicalProgram, Solve mp = MathematicalProgram() u = mp.NewContinuousVariables(self.nu, "u") qdd = mp.NewContinuousVariables(self.nq, "qdd") x_y_dimensions = 2 lambda_variable = mp.NewContinuousVariables(x_y_dimensions, number_of_grasp_points, "lambda_variable") forces = np.zeros((self.nq)) for i in range(number_of_grasp_points): current_forces = np.transpose(J_contact)[:,i,:].dot(lambda_variable[:,i]) forces = forces + current_forces leftHandSide = M.dot(qdd) + C rightHandSide = B.dot(u) + forces for i in range(len(leftHandSide)): mp.AddConstraint(leftHandSide[i] == rightHandSide[i]) # Calculate Normals normals = np.array([]) for i in range(number_of_grasp_points): current_grasp_normal = grasp_normals_world_now[0:2, i] normals = np.vstack((normals, current_grasp_normal)) if normals.size else current_grasp_normal # Calculate Tangeants tangeants = np.array([]) for i in range(number_of_grasp_points): current_grasp_tangeant = np.array([normals[i, 1], -normals[i, 0]]) tangeants = np.vstack((tangeants, current_grasp_tangeant)) if tangeants.size else current_grasp_tangeant # Create beta variables beta = mp.NewContinuousVariables(2, number_of_grasp_points, "b0") # Create Grasps for i in range(number_of_grasp_points): c0 = normals[i] - self.mu * tangeants[i] c1 = normals[i] + self.mu * tangeants[i] right_hand_lambda1 = c0 * beta[0, i] + c1 * beta[1, i] mp.AddConstraint(lambda_variable[0, i] == right_hand_lambda1[0]) mp.AddConstraint(lambda_variable[1, i] == right_hand_lambda1[1]) mp.AddConstraint(beta[0, i] >= 0) mp.AddConstraint(beta[1, i] >= 0) mp.AddConstraint(normals[i].dot(lambda_variable[:, i]) >= 0.25) # Copying the control period of the constructor. Probably not supposed to do this... next_tick_qd = v + qdd * self.cont¨rol_period next_tick_q = q + next_tick_qd * self.control_period q_error = qdes - next_tick_q proportionalCost = q_error.dot(np.transpose(q_error)) qd_error = 0 - next_tick_qd diffCost = qd_error.dot(np.transpose(qd_error)) mp.AddQuadraticCost(kp * proportionalCost + kd * diffCost) result = Solve(mp) u_solution = result.GetSolution(u) u = np.zeros(self.nu) return u_solution
class Optimization: def __init__(self, config): self.physical_params = config["physical_params"] self.T = config["T"] self.dt = config["dt"] self.initial_state = config["xinit"] self.goal_state = config["xgoal"] self.ellipse_arc = config["ellipse_arc"] self.deviation_cost = config["deviation_cost"] self.Qf = config["Qf"] self.limits = config["limits"] self.n_state = 6 self.n_nominal_forces = 4 self.tire_model = config["tire_model"] self.initial_guess_config = config["initial_guess_config"] self.puddle_model = config["puddle_model"] self.force_constraint = config["force_constraint"] self.visualize_initial_guess = config["visualize_initial_guess"] self.dynamics = Dynamics(self.physical_params.lf, self.physical_params.lr, self.physical_params.m, self.physical_params.Iz, self.dt) def build_program(self): self.prog = MathematicalProgram() # Declare variables. state = self.prog.NewContinuousVariables(rows=self.T + 1, cols=self.n_state, name='state') nominal_forces = self.prog.NewContinuousVariables( rows=self.T, cols=self.n_nominal_forces, name='nominal_forces') steers = self.prog.NewContinuousVariables(rows=self.T, name="steers") slip_ratios = self.prog.NewContinuousVariables(rows=self.T, cols=2, name="slip_ratios") self.state = state self.nominal_forces = nominal_forces self.steers = steers self.slip_ratios = slip_ratios # Set the initial state. xinit = pack_state_vector(self.initial_state) for i, s in enumerate(xinit): self.prog.AddConstraint(state[0, i] == s) # Constrain xdot to always be at least some value to prevent numerical issues with optimizer. for i in range(self.T + 1): s = unpack_state_vector(state[i]) self.prog.AddConstraint(s["xdot"] >= self.limits.min_xdot) # Constrain slip ratio to be at least a certain magnitude. if i != self.T: self.prog.AddConstraint( slip_ratios[i, 0]**2.0 >= self.limits.min_slip_ratio_mag**2.0) self.prog.AddConstraint( slip_ratios[i, 1]**2.0 >= self.limits.min_slip_ratio_mag**2.0) # Control rate limits. for i in range(self.T - 1): ddelta = self.dt * (steers[i + 1] - steers[i]) f_dkappa = self.dt * (slip_ratios[i + 1, 0] - slip_ratios[i, 0]) r_dkappa = self.dt * (slip_ratios[i + 1, 1] - slip_ratios[i, 1]) self.prog.AddConstraint(ddelta <= self.limits.max_ddelta) self.prog.AddConstraint(ddelta >= -self.limits.max_ddelta) self.prog.AddConstraint(f_dkappa <= self.limits.max_dkappa) self.prog.AddConstraint(f_dkappa >= -self.limits.max_dkappa) self.prog.AddConstraint(r_dkappa <= self.limits.max_dkappa) self.prog.AddConstraint(r_dkappa >= -self.limits.max_dkappa) # Control value limits. for i in range(self.T): self.prog.AddConstraint(steers[i] <= self.limits.max_delta) self.prog.AddConstraint(steers[i] >= -self.limits.max_delta) # Add dynamics constraints by constraining residuals to be zero. for i in range(self.T): residuals = self.dynamics.nominal_dynamics(state[i], state[i + 1], nominal_forces[i], steers[i]) for r in residuals: self.prog.AddConstraint(r == 0.0) # Add the cost function. self.add_cost(state) # Add a different force constraint depending on the configuration. if self.force_constraint == ForceConstraint.NO_PUDDLE: self.add_no_puddle_force_constraints( state, nominal_forces, steers, self.tire_model.get_deterministic_model(), slip_ratios) elif self.force_constraint == ForceConstraint.MEAN_CONSTRAINED: self.add_mean_constrained() elif self.force_constraint == ForceConstraint.CHANCE_CONSTRAINED: self.add_chance_constraints() else: raise NotImplementedError("ForceConstraint type invalid.") return def constant_input_initial_guess(self, state, steers, slip_ratios, nominal_forces): # Guess the slip ratio as the minimum allowed value. gslip_ratios = np.tile( np.array([ self.limits.min_slip_ratio_mag, self.limits.min_slip_ratio_mag ]), (self.T, 1)) # Guess the slip angle as a linearly ramping steer that then becomes constant. # This is done by creating an array of values corresponding to end_delta then # filling in the initial ramp up phase. gsteers = self.initial_guess_config["end_delta"] * np.ones(self.T) igc = self.initial_guess_config for i in range(igc["ramp_steps"]): gsteers[i] = (i / igc["ramp_steps"]) * (igc["end_delta"] - igc["start_delta"]) # Create empty arrays for state and forces. gstate = np.empty((self.T + 1, self.n_state)) gstate[0] = pack_state_vector(self.initial_state) gforces = np.empty((self.T, 4)) all_slips = self.T * [None] # Simulate the dynamics for the initial guess differently depending on the force # constraint being used. if self.force_constraint == ForceConstraint.NO_PUDDLE: tire_model = self.tire_model.get_deterministic_model() for i in range(self.T): s = unpack_state_vector(gstate[i]) # Simulate the dynamics for one time step. gstate[i + 1], forces, slips = self.dynamics.simulate( gstate[i], gsteers[i], gslip_ratios[i, 0], gslip_ratios[i, 1], tire_model, self.dt) # Store the results. gforces[i] = pack_force_vector(forces) all_slips[i] = slips elif self.force_constraint == ForceConstraint.MEAN_CONSTRAINED or self.force_constraint == ForceConstraint.CHANCE_CONSTRAINED: # mean model is a function that maps (slip_ratio, slip_angle, x, y) -> (E[Fx], E[Fy]) mean_model = self.tire_model.get_mean_model( self.puddle_model.get_mean_fun()) for i in range(self.T): # Update the tire model based off the conditions of the world # at (x, y) s = unpack_state_vector(gstate[i]) tire_model = lambda slip_ratio, slip_angle: mean_model( slip_ratio, slip_angle, s["X"], s["Y"]) # Simulate the dynamics for one time step. gstate[i + 1], forces, slips = self.dynamics.simulate( gstate[i], gsteers[i], gslip_ratios[i, 0], gslip_ratios[i, 1], tire_model, self.dt) # Store the results. gforces[i] = pack_force_vector(forces) all_slips[i] = slips else: raise NotImplementedError( "Invalid value for self.force_constraint") # Declare array for the initial guess and set the values. initial_guess = np.empty(self.prog.num_vars()) self.prog.SetDecisionVariableValueInVector(state, gstate, initial_guess) self.prog.SetDecisionVariableValueInVector(steers, gsteers, initial_guess) self.prog.SetDecisionVariableValueInVector(slip_ratios, gslip_ratios, initial_guess) self.prog.SetDecisionVariableValueInVector(nominal_forces, gforces, initial_guess) if self.visualize_initial_guess: # TODO: reorganize visualizations psis = gstate[:, 2] xs = gstate[:, 4] ys = gstate[:, 5] fig, ax = plt.subplots() if self.force_constraint != ForceConstraint.NO_PUDDLE: plot_puddles(ax, self.puddle_model) plot_ellipse_arc(ax, self.ellipse_arc) plot_planned_trajectory(ax, xs, ys, psis, gsteers, self.physical_params) # Plot the slip ratios/angles plot_slips(all_slips) plot_forces(gforces) if self.force_constraint == ForceConstraint.NO_PUDDLE: generate_animation(xs, ys, psis, gsteers, self.physical_params, self.dt, puddle_model=None) else: generate_animation(xs, ys, psis, gsteers, self.physical_params, self.dt, puddle_model=self.puddle_model) return initial_guess def solve_program(self): # Generate initial guess initial_guess = self.constant_input_initial_guess( self.state, self.steers, self.slip_ratios, self.nominal_forces) # Solve the problem. solver = SnoptSolver() result = solver.Solve(self.prog, initial_guess) solver_details = result.get_solver_details() print("Exit flag: " + str(solver_details.info)) self.visualize_results(result) def visualize_results(self, result): state_res = result.GetSolution(self.state) psis = state_res[:, 2] xs = state_res[:, 4] ys = state_res[:, 5] steers = result.GetSolution(self.steers) fig, ax = plt.subplots() plot_ellipse_arc(ax, self.ellipse_arc) plot_puddles(ax, self.puddle_model) plot_planned_trajectory(ax, xs, ys, psis, steers, self.physical_params) generate_animation(xs, ys, psis, steers, self.physical_params, self.dt, puddle_model=self.puddle_model) plt.show() def add_cost(self, state): # Add the final state cost function. diff_state = pack_state_vector(self.goal_state) - state[-1] self.prog.AddQuadraticCost(diff_state.T @ self.Qf @ diff_state) # Get the approx distance function for the ellipse. fun = self.ellipse_arc.approx_dist_fun() for i in range(self.T): s = unpack_state_vector(state[i]) self.prog.AddCost(self.deviation_cost * fun(s["X"], s["Y"])) def add_mean_constrained(self): # mean model is a function that maps (slip_ratio, slip_angle, x, y) -> (E[Fx], E[Fy]) mean_model = self.tire_model.get_mean_model( self.puddle_model.get_mean_fun()) for i in range(self.T): # Get slip angles and slip ratios. s = unpack_state_vector(self.state[i]) F = unpack_force_vector(self.nominal_forces[i]) # get the tire model at this position in space. tire_model = lambda slip_ratio, slip_angle: mean_model( slip_ratio, slip_angle, s["X"], s["Y"]) # Unpack values delta = self.steers[i] alpha_f, alpha_r = self.dynamics.slip_angles( s["xdot"], s["ydot"], s["psidot"], delta) kappa_f = self.slip_ratios[i, 0] kappa_r = self.slip_ratios[i, 1] # Compute expected forces. E_Ffx, E_Ffy = tire_model(kappa_f, alpha_f) E_Frx, E_Fry = tire_model(kappa_r, alpha_r) # Constrain these expected force values to be equal to the nominal # forces in the optimization. self.prog.AddConstraint(E_Ffx - F["f_long"] == 0.0) self.prog.AddConstraint(E_Ffy - F["f_lat"] == 0.0) self.prog.AddConstraint(E_Frx - F["r_long"] == 0.0) self.prog.AddConstraint(E_Fry - F["r_lat"] == 0.0) def add_chance_constrained(self): pass def add_no_puddle_force_constraints(self, state, forces, steers, pacejka_model, slip_ratios): """ Args: prog: state: forces: steers: pacejka_model: function with signature (slip_ratio, slip_angle) using pydrake.math """ for i in range(self.T): # Get slip angles and slip ratios. s = unpack_state_vector(state[i]) F = unpack_force_vector(forces[i]) delta = steers[i] alpha_f, alpha_r = self.dynamics.slip_angles( s["xdot"], s["ydot"], s["psidot"], delta) kappa_f = slip_ratios[i, 0] kappa_r = slip_ratios[i, 1] Ffx, Ffy = pacejka_model(kappa_f, alpha_f) Frx, Fry = pacejka_model(kappa_r, alpha_r) # Constrain the values from the pacejka model to be equal # to the desired values in the optimization. self.prog.AddConstraint(Ffx - F["f_long"] == 0.0) self.prog.AddConstraint(Ffy - F["f_lat"] == 0.0) self.prog.AddConstraint(Frx - F["r_long"] == 0.0) self.prog.AddConstraint(Fry - F["r_lat"] == 0.0)
def compute_trajectory_to_other_world(self, state_initial, minimum_time, maximum_time): ''' Your mission is to implement this function. A successful implementation of this function will compute a dynamically feasible trajectory which satisfies these criteria: - Efficiently conserve fuel - Reach "orbit" of the far right world - Approximately obey the dynamic constraints - Begin at the state_initial provided - Take no more than maximum_time, no less than minimum_time The above are defined more precisely in the provided notebook. Please note there are two return args. :param: state_initial: :param state_initial: numpy array of length 4, see rocket_dynamics for documentation :param: minimum_time: float, minimum time allowed for trajectory :param: maximum_time: float, maximum time allowed for trajectory :return: three return args separated by commas: trajectory, input_trajectory, time_array trajectory: a 2d array with N rows, and 4 columns. See simulate_states_over_time for more documentation. input_trajectory: a 2d array with N-1 row, and 2 columns. See simulate_states_over_time for more documentation. time_array: an array with N rows. ''' # length of horizon (excluding init state) N = 50 trajectory = np.zeros((N + 1, 4)) input_trajectory = np.ones((N, 2)) * 10.0 ### My implementation of Direct Transcription # (states and control are all decision vars using Euler integration) mp = MathematicalProgram() # let trajectory duration be a decision var total_time = mp.NewContinuousVariables(1, "total_time") dt = total_time[0] / N # create the control decision var (m*N) and state decision var (n*[N+1]) idx = 0 u_list = mp.NewContinuousVariables(2, "u_{}".format(idx)) state_list = mp.NewContinuousVariables(4, "state_{}".format(idx)) state_list = np.vstack( (state_list, mp.NewContinuousVariables(4, "state_{}".format(idx + 1)))) for idx in range(1, N): u_list = np.vstack( (u_list, mp.NewContinuousVariables(2, "u_{}".format(idx)))) state_list = np.vstack( (state_list, mp.NewContinuousVariables(4, "state_{}".format(idx + 1)))) ### Constraints # set init state as constraint on stage 0 decision vars for state_idx in range(4): mp.AddLinearConstraint( state_list[0, state_idx] == state_initial[state_idx]) # interstage equality constraint on state vars via Euler integration # note: Direct Collocation could improve accuracy for same computation for idx in range(1, N + 1): state_new = state_list[idx - 1, :] + dt * self.rocket_dynamics( state_list[idx - 1, :], u_list[idx - 1, :]) for state_idx in range(4): mp.AddConstraint(state_list[idx, state_idx] == state_new[state_idx]) # constraint on time mp.AddLinearConstraint(total_time[0] <= maximum_time) mp.AddLinearConstraint(total_time[0] >= minimum_time) # constraint on final state distance (squared)to second planet final_dist_to_world_2_sq = ( self.world_2_position[0] - state_list[-1, 0])**2 + ( self.world_2_position[1] - state_list[-1, 1])**2 mp.AddConstraint(final_dist_to_world_2_sq <= 0.25) # constraint on final state speed (squared final_speed_sq = state_list[-1, 2]**2 + state_list[-1, 3]**2 mp.AddConstraint(final_speed_sq <= 1) ### Cost # equal cost on vertical/horizontal accels, weight shouldn't matter since this is the only cost mp.AddQuadraticCost(1 * u_list[:, 0].dot(u_list[:, 0])) mp.AddQuadraticCost(1 * u_list[:, 1].dot(u_list[:, 1])) ### Solve and parse result = Solve(mp) trajectory = result.GetSolution(state_list) input_trajectory = result.GetSolution(u_list) tf = result.GetSolution(total_time) time_array = np.linspace(0, tf[0], N + 1) print "optimization successful: ", result.is_success() print "total num decision vars (x: (N+1)*4, u: 2N, total_time: 1): {}".format( mp.num_vars()) print "solver used: ", result.get_solver_id().name() print "optimal trajectory time: {:.2f} sec".format(tf[0]) return trajectory, input_trajectory, time_array
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