def solve_BSP(self, robot, X0, cov_X0, Ui, U_guess): #Belief space planning opti = c.Opti() U = opti.variable(robot.nu * self.T, 1) opti.set_initial(U, U_guess) opti.minimize(self.cost_func_BSP(robot, U, X0, cov_X0)) #control constraints opti.subject_to(U <= self.U_upper_bound(robot)) opti.subject_to(U >= self.U_lower_bound(robot)) #state constraints opti.subject_to(self.state_contraints(robot, U) > 0) opts = {} opts['ipopt.print_level'] = 0 opti.solver('ipopt', opts) sol = opti.solve() U_opti = sol.value(U) U_opti = c.reshape(U_opti, robot.nu, self.T) return U_opti
def __init__(self, state_probabilities, asset_values): self.state_probabilities = state_probabilities self.asset_values = asset_values self.validate() n_states = self.n_states n_assets = self.n_assets self.opti = opti = casadi.Opti() self.expressions = {} p_asset_values = opti.parameter(n_states, n_assets) opti.set_value(p_asset_values, asset_values) p_state_probabilities = opti.parameter(n_states) opti.set_value(p_state_probabilities, state_probabilities) v_coded = opti.variable(n_assets) coded_total = casadi.sum1(v_coded) asset_weights = v_coded / (coded_total + 1) # the rest is cash cash = 1.0 / (coded_total + 1) # 1-sum(weights) state_values = casadi.mtimes(p_asset_values, asset_weights) + cash kelly_criterion = casadi.dot(casadi.log(state_values), p_state_probabilities) opti.minimize(-kelly_criterion) opti.subject_to(v_coded >= 0) opti.set_initial(v_coded, np.ones(n_assets)) opti.solver('ipopt') self.expressions = DS({ name: value for name, value in locals().items() if isinstance(value, casadi.MX) })
def solve_SSP(self, robot, X0, Ui, U_guess): opti = c.Opti() U = opti.variable(robot.nu * self.T, 1) opti.set_initial(U, U_guess) opti.minimize(self.cost_func_SSP(robot, U, X0)) #control constraints opti.subject_to(U <= self.U_upper_bound(robot)) opti.subject_to(U >= self.U_lower_bound(robot)) #state constraints #opti.subject_to(self.state_contraints(robot,U) > 0) p_opts = {} p_opts['ipopt.print_level'] = 0 s_opts = {"max_iter": 100} opti.solver('ipopt', p_opts, s_opts) try: sol = opti.solve() U_opti = sol.value(U) except RuntimeError: U_opti = opti.debug.value(U) print "debug value used." # sol = opti.solve() # U_opti = sol.value(U) U_opti = c.reshape(U_opti, robot.nu, self.T) return U_opti
def closest_point_between_line_and_circle(x1, x2, y1, y2, x_center, y_center, radius): """ THIS IS AN OPTIMIZATION PROBLEM THROUGH AND THROUGH""" """ FIND THE CLOSEST POINT BETWEEN A LINE AND A CIRCLE""" import casadi as ca opti = ca.Opti() # Line segment t = opti.variable() line_seg = opti.bounded(0, t, 1) p0 = (x1, y1) p1 = (x1, y2) x = (1 - t) * p0 + t * p1 # Circle y = opti.variable(2) circ = y[0]**2 + y[1]**2 == 1 # Optimization problem dist_squared = (x[0] - y[0])**2 + (x[1] - y[1])**2 opti.minimize(dist_squared) opti.subject_to(line_seg) opti.subject_to(circ) # Solve opti.solver('ipopt') sol = opti.solve() # Result print(f"Distance = {sol.value(ca.sqrt(dist_squared))}") print(f"Point on line segment: {sol.value(x)}") print(f"Point on circle: {sol.value(y)}")
def step(self, s, target_position, time=None): self.s = s self.target_position = target_position opti = casadi.Opti() Q = opti.variable(len(self.Q_hat0)) opti.minimize(self.cost_function(Q)) opti.subject_to(Q <= 1) opti.subject_to(Q >= -1) opti.solver('ipopt', {}, { 'linear_solver': 'ma27', 'print_level': 0, 'sb': 'yes' }) try: sol = opti.solve() except: print(opti.debug.value(Q)) self.Q_hat0 = np.zeros(self.mpc_horizon) return self.Q_previous self.Q_hat = sol.value(Q) # self.plot_prediction() # Prepare guess for next iteration self.Q_hat0 = np.hstack((self.Q_hat[1:], self.Q_hat[-1])) # self.Q_hat0 = np.zeros(self.mpc_horizon) # Working also in this version Q = self.Q_hat[0] return Q
def __init__(self, knot_points_per_phase, steps, total_duration, model='hopper', terrain='flat'): super().__init__() self.knot_points_per_phase = knot_points_per_phase self.num_phases = steps self.total_duration = total_duration if model == 'hopper': self.model = Hopper() self.time_phases = {} self.terrain = Terrain(type=terrain) self.opti = ca.Opti() self.q = {} self.qdot = {} self.u = {} self.p0 = {} # i = 0 self.dp0 = {} # i = 0 self.f10 = {} # i = 0 self.ceq = [] self.ciq = [] self.setVariables() self.setConstraints() self.setBounds()
def estimate(self, rt, s, c): self.model = casadi.Opti() e = self.model.variable(1, 1) er_l1 = self.model.variable(1, len(rt)) self.model.subject_to(e >= 0) self.model.subject_to(er_l1 >= 0) obj = 0 for i in range(len(rt)): if (c[i] < s[i]): self.model.subject_to(er_l1[0, i] >= rt[i] - e) self.model.subject_to(er_l1[0, i] >= -rt[i] + e) else: self.model.subject_to(er_l1[0, i] >= rt[i] - (c[i] / s[i]) * e) self.model.subject_to(er_l1[0, i] >= -rt[i] + (c[i] / s[i]) * e) obj += er_l1[0, i] self.model.minimize(obj) optionsIPOPT = {'print_time': False, 'ipopt': {'print_level': 0}} self.model.solver('ipopt', optionsIPOPT) sol = self.model.solve() return sol.value(e)
def __init__(self, n_control_step, num_states=4, num_controls=2): self.num_control_steps = n_control_step self.num_states = num_states self.num_controls = num_controls self.opti = casadi.Opti() self.setup_vars() self.setup_defect() self.sol = None
def __init__(self, start, start_pos, start_vel=[0., 0., 0., 0., 0.]): # set our parameters of optimization self.opti = ca.Opti() self.terrain_factor = 0. self.N = 40 self.T = 0.1 self.step_max = 0.2 self.tauMax = 1000 self.pi = np.pi self.l1 = 0.5 self.l2 = 0.5 self.l3 = 0.5 self.m = [0.25, 0.25, 0.25, 0.25, 0.25] #; self.m2 = 0.5; self.m3 = 0.5 self.i1 = self.m[0] * (self.l1**2) / 12 self.i2 = self.m[1] * (self.l2**2) / 12 self.i3 = self.m[2] * (self.l3**2) / 12 self.i = [self.i1, self.i2, self.i3, self.i2, self.i1] self.g = 9.81 self.h = self.T / self.N self.comh = 0.05 goali = start goalf = goali[::-1] self.goal = [goali, goalf] self.goal_vel = [start_vel, start_vel[::-1]] if start_pos == [[0, 0]]: self.p0 = self.heightMap(start_pos) else: self.p0 = start_pos #set our optimization variables self.state = [] self.u = [] for i in range(self.N): rowu = [] rowq = [] rowq.append(self.opti.variable(5)) rowq.append(self.opti.variable(5)) rowu.append(self.opti.variable(4)) self.state.append(rowq) self.u.append(rowu) self.pos = [] self.com = [] self.ddq = [] for i in range(self.N): p, dp, g, dg, ddq = self.getModel(self.state[i], self.u[i]) self.pos.append(p) self.com.append(g) self.ddq.append(ddq) if i == 0: # self.impactmap = self.heelStrike(self.state[i][0],self.state[i][1],p,dp,g,dg) self.dp0 = dp if i == self.N - 1: self.dpN = dp self.impactmap = self.heelStrike(self.state[i][0], self.state[i][1], p, dp, g, dg)
def iterative_optimization(path_function, start, v, s): """ OPTIMIZATION """ K = 9 #time steps opti = casadi.Opti() V = opti.variable(K, 1) S = opti.variable(K, 1) cost_function(V, S, start, v, s, path_function)
def buildOpt(self, MU, P, S, dt, tgt, H): self.model = casadi.Opti() self.sVar = self.model.variable(P.shape[0], 1) self.pVar = self.model.variable(P.shape[0], P.shape[1]) self.stateVar = self.model.variable(P.shape[0], H) self.tVar = self.model.variable(P.shape[0], H) self.initX = self.model.parameter(P.shape[0], 1) # self.E_abs = self.model.variable(1,H) #self.model.subject_to(self.model.bounded(0.0,self.sVar,3000)) self.model.subject_to(self.sVar >= 0) self.model.subject_to(self.model.bounded(0, casadi.vec(self.pVar), 1)) for i in range(P.shape[0]): if (S[i, 0] >= 0): self.model.subject_to(self.sVar[i] == S[i, 0]) for j in range(P.shape[1]): if (P[i, j] >= 0): self.model.subject_to(self.pVar[i, j] == P[i, j]) #self.model.subject_to((self.pVar[i,0]+self.pVar[i,1]+self.pVar[i,2])==1.0) self.model.subject_to(self.stateVar[:, 0] == self.initX) for i in range(P.shape[0]): for h in range(H): self.model.subject_to(self.sVar[1, 0] <= self.stateVar[1, h]) if (i == 0): self.model.subject_to(self.tVar[i, h] == MU[i]) else: #self.model.subject_to(self.tVar[i,h]==MU[i]*casadi.fmin(self.sVar[i,0],self.stateVar[i,h])) self.model.subject_to(self.tVar[i, h] == MU[i] * self.sVar[i, 0]) for i in range(P.shape[0]): for h in range(H - 1): self.model.subject_to( self.stateVar[i, h + 1] == (-self.tVar[i, h] + self.pVar[:, i].T @ self.tVar[:, h]) * dt + self.stateVar[i, h]) # self.model.subject_to(self.stateVar[i,h+1]==(-MU[i]*casadi.fmin(self.sVar[i],self.stateVar[i,h]) # +self.pVar[:,i].T@(MU*casadi.fmin(self.sVar,self.stateVar[:,h])))*dt+self.stateVar[i,h]) # for h in range(H): # self.model.subject_to(self.E_abs[0,h]>=(self.stateVar[1,h]-tgt*(1.0/MU[1])*self.tVar[1,h])) # self.model.subject_to(self.E_abs[0,h]>=-(self.stateVar[1,h]-tgt*(1.0/MU[1])*self.tVar[1,h])) #self.model.minimize(casadi.sumsqr(self.stateVar[1,:]-tgt)) # obj=0 # for h in range(H): # obj+=(self.stateVar[1,h]-tgt*MU[1]*self.tVar[1,h])**2 optionsIPOPT = {'print_time': False, 'ipopt': {'print_level': 0}} optionsOSQP = {'print_time': False, 'osqp': {'verbose': False}} self.model.solver('ipopt', optionsIPOPT)
def __init__(self): super().__init__() self.model = Hopper() self.__setOptimizationParams__(total_duration=0.4, epsilon=0.) self.opti = ca.Opti() self.var_dt = False self.__setVariables__() self.__setConstraints__() self.__setCosts__()
def __init__(self, start_angles, start_angular_vel, start_pos): # set our parameters of optimization self.opti = ca.Opti() self.terrain_factor = 0.1 self.N = 40 self.T = 0.3 self.step_max = 0.5 self.tauMax = 20 self.pi = np.pi self.length = ca.MX([0.5, 0.5, 0.5, 0.5, 0.5]) self.mass = ca.MX([0.25, 0.25, 0.25, 0.25, 0.25]) self.inertia = self.mass * (self.length**2) / 12 self.gravity = 9.81 self.h = self.T / self.N self.goal = [start_angles, start_angular_vel] self.ini_goal = self.goal[0].to_DM() self.fin_goal = ca.DM([ self.ini_goal[4], self.ini_goal[3], self.ini_goal[2], self.ini_goal[1], self.ini_goal[0] ]) self.p0 = ca.MX(start_pos) self.comh = self.length[0] * 0.5 #set our optimization variables self.x = [] self.xdot = [] self.u = [] self.left = [] self.right = [] for i in range(self.N): self.x.append(self.opti.variable(5)) self.xdot.append(self.opti.variable(5)) self.u.append(self.opti.variable(4)) self.left.append(self.opti.variable(2)) self.right.append(self.opti.variable(2)) self.pos = [] self.com = [] self.ddq = [] self.dpos = [] for n in range(self.N): p, dp, ddp, c, dc, ddc = self.getKinematics( self.x[n], self.xdot[n]) self.pos.append(p) self.dpos.append(dp) self.com.append(c) ddq = self.getDynamics(self.x[n], self.xdot[n], self.u[n], p, ddp, c, ddc) self.ddq.append(ddq) if n == self.N - 1: self.x_impact, self.xdot_impact = self.impactMap( self.x[n], self.xdot[n], p, dp, c, dc)
def __init__(self, ts=0.005): self.variables_dot = [] #derivative of the variables self.lb = [] #variable_dot lower bounds self.ub = [] #variable_dot upper bounds self.variables0 = [ ] #Expression for the current value of the variables self.constraints = {} #dictionary of constraints at priority levels self.opti = cs.Opti() #creating CasADi's optistack self.once_solved = False self.number_priority_levels = 0 self.time_taken = 0
def __init__(self): super().__init__() self.model = FingerContact() self.__setOptimizationParams__(total_duration=2, n_steps=20, epsilon=1e-4) self.opti = ca.Opti() self.var_dt = True self.__setVariables__() self.__setConstraints__() self.__setCosts__()
def get_optimal_path( path, robot, scene=None, q_init=None, max_iters=100, w=None, cow=0.0 ): N = len(path) if q_init is None: q_init = np.zeros((N, robot.ndof)) if w is None: w = np.ones(robot.ndof) print("Using weights:") print(w) opti = ca.Opti() q = opti.variable(N, robot.ndof) # joint variables along path # collision constraints if scene is not None: cons = create_cc(opti, robot, scene, q) opti.subject_to(cons) # path constraints opti.subject_to(create_path_constraints(q, robot, path)) # objective # V = ca.sum1( # ca.sum2((q[:-1, :] - q[1:, :]) ** 2) # ) # + 0.05* ca.sumsqr(q) #+ 1 / ca.sum1(q[:, 4]**2) V = 0 for i in range(1, N): for k in range(robot.ndof): V += w[k] * (q[i, k] - q[i - 1, k]) ** 2 if cow > 0: print(f"Adding path constraints objective term with lambda: {cow}") V += cow * create_path_objective(q, robot, path) opti.minimize(V) p_opts = {} # casadi options s_opts = {"max_iter": max_iters} # solver options opti.solver("ipopt", p_opts, s_opts) opti.set_initial(q, q_init) # 2 3 4 5 converges try: sol = opti.solve() except RuntimeError as e: print(e) # return opti object to access debug info return Solution(False, extra_info={"opti": opti}) if sol.stats()["success"]: return Solution(True, sol.value(q), sol.value(V)) else: return Solution(False)
def mpc_lti(xcurv, xtarget, mpc_lti_param, system_param, track): vt = xtarget[0] eyt = xtarget[5] num_horizon = mpc_lti_param.num_horizon start_timer = datetime.datetime.now() opti = ca.Opti() # define variables xvar = opti.variable(X_DIM, num_horizon + 1) uvar = opti.variable(U_DIM, num_horizon) cost = 0 opti.subject_to(xvar[:, 0] == xcurv) # dynamics + state/input constraints for i in range(num_horizon): # system dynamics opti.subject_to( xvar[:, i + 1] == ca.mtimes(mpc_lti_param.matrix_A, xvar[:, i]) + ca.mtimes(mpc_lti_param.matrix_B, uvar[:, i])) # min and max of delta opti.subject_to(-system_param.delta_max <= uvar[0, i]) opti.subject_to(uvar[0, i] <= system_param.delta_max) # min and max of a opti.subject_to(-system_param.a_max <= uvar[1, i]) opti.subject_to(uvar[1, i] <= system_param.a_max) # input cost cost += ca.mtimes(uvar[:, i].T, ca.mtimes(mpc_lti_param.matrix_R, uvar[:, i])) for i in range(num_horizon + 1): # speed vx upper bound opti.subject_to(xvar[0, i] <= system_param.v_max) opti.subject_to(xvar[0, i] >= system_param.v_min) # min and max of ey opti.subject_to(xvar[5, i] <= track.width) opti.subject_to(-track.width <= xvar[5, i]) # state cost cost += ca.mtimes( (xvar[:, i] - xtarget).T, ca.mtimes(mpc_lti_param.matrix_Q, xvar[:, i] - xtarget), ) # setup solver option = {"verbose": False, "ipopt.print_level": 0, "print_time": 0} opti.minimize(cost) opti.solver("ipopt", option) sol = opti.solve() x_pred = sol.value(xvar).T u_pred = sol.value(uvar).T end_timer = datetime.datetime.now() solver_time = (end_timer - start_timer).total_seconds() print("solver time: {}".format(solver_time)) return u_pred[0, :]
def information_gain(): opti = casadi.Opti() B = 0.5 ui = 0 u = [] #This is the initial training point N = len( u ) + 1 #The amount of previous training data (so we see if we are actually learning anything new). This is not usually a constant I = 0 for i in range(0, N): u.append(opti.variable()) #uk if i != 0: I += casadi.norm_1(u[-1] - u[-2]) else: I += casadi.norm_1(u[-1] - ui)
def __init__(self): # set our parameters of optimization self.opti = ca.Opti() self.N = 100 self.T = 0.1 self.step_max = 1 self.tauMax = 1.5 self.pi = np.pi self.l1 = 0.5 self.l2 = 0.5 self.l3 = 0.6 self.m = [0.5, 0.5, 0.5, 0.5, 0.5] #; self.m2 = 0.5; self.m3 = 0.5 self.i1 = self.m[0] * (self.l1**2) / 12 self.i2 = self.m[1] * (self.l2**2) / 12 self.i3 = self.m[2] * (self.l3**2) / 12 self.i = [self.i1, self.i2, self.i3, self.i2, self.i1] self.g = -9.81 self.h = self.T / self.N goali = [-0.3, 0.7, 0.0, -0.5, -0.3] goalf = goali[::-1] self.goal = [goali, goalf] #set our optimization variables self.state = [] self.u = [] for i in range(self.N): rowu = [] rowq = [] rowq.append(self.opti.variable(5)) rowq.append(self.opti.variable(5)) rowu.append(self.opti.variable(4)) self.state.append(rowq) self.u.append(rowu) self.pos = [] self.com = [] self.ddq = [] for i in range(self.N): p, dp, g, dg, ddq = self.getModel(self.state[i], self.u[i]) self.pos.append(p) self.com.append(g) self.ddq.append(ddq) if i == 0: self.dp0 = dp if i == self.N - 1: self.dpN = dp self.impactmap = self.heelStrike(self.state[i][0], self.state[i][1], p, dp, g, dg)
def __init__(self, knot_points_per_phase, steps, total_duration, model='biped', terrain='flat'): super().__init__() self.knot_points_per_phase = knot_points_per_phase self.num_phases = steps self.total_duration = total_duration if model == 'biped': self.model = Biped1() self.time_phases = {'Left Leg': {}, 'Right Leg': {}} self.terrain = Terrain(type=terrain) self.opti = ca.Opti() self.lq = {} self.lqdot = {} # self.lqddot = {} self.rq = {} self.rqdot = {} # self.rqddot = {} self.tq = {} self.tqdot = {} # self.tqddot = {} self.u = {} self.lpos = {} # i = 0 self.dlpos = {} # i = 0 self.ddlpos = {} # i = 0 self.lforce = {} # i = 0 self.rpos = {} # i = 1 self.drpos = {} # i = 1 self.ddrpos = {} # i = 1 self.rforce = {} # i = 1 self.setVariables() self.setConstraints()
def calc_input(self): xt = np.array([self.vt, 0, 0, 0, 0, self.eyt]).reshape(self.xdim, 1) start_timer = datetime.datetime.now() opti = ca.Opti() # define variables xvar = opti.variable(self.xdim, self.num_of_horizon + 1) uvar = opti.variable(self.udim, self.num_of_horizon) cost = 0 opti.subject_to(xvar[:, 0] == self.x) # dynamics + state/input constraints for i in range(self.num_of_horizon): # system dynamics opti.subject_to(xvar[:, i + 1] == ca.mtimes(self.matrix_A, xvar[:, i]) + ca.mtimes(self.matrix_B, uvar[:, i])) # min and max of delta opti.subject_to(-0.5 <= uvar[0, i]) opti.subject_to(uvar[0, i] <= 0.5) # min and max of a opti.subject_to(-1.0 <= uvar[1, i]) opti.subject_to(uvar[1, i] <= 1.0) # input cost cost += ca.mtimes(uvar[:, i].T, ca.mtimes(self.matrix_R, uvar[:, i])) for i in range(self.num_of_horizon + 1): # speed vx upper bound opti.subject_to(xvar[0, i] <= 10.0) opti.subject_to(xvar[0, i] >= 0.0) # min and max of ey opti.subject_to(xvar[5, i] <= 2.0) opti.subject_to(-2.0 <= xvar[5, i]) # state cost cost += ca.mtimes((xvar[:, i] - xt).T, ca.mtimes(self.matrix_Q, xvar[:, i] - xt)) # setup solver option = {"verbose": False, "ipopt.print_level": 0, "print_time": 0} opti.minimize(cost) opti.solver("ipopt", option) sol = opti.solve() end_timer = datetime.datetime.now() solver_time = (end_timer - start_timer).total_seconds() print("solver time: {}".format(solver_time)) self.x_pred = sol.value(xvar).T self.u_pred = sol.value(uvar).T self.u = self.u_pred[0, :] self.time += self.timestep
def ComputeTerminalRegion(the_A, the_B, the_Q, the_R, the_umin, the_umax): # step 1: determine K using LQR P = np.matrix(scipy.linalg.solve_continuous_are(the_A, the_B, the_Q, the_R)) K = np.array(scipy.linalg.inv(the_R) * (the_B.T * P)) eigvals, eigvecs = np.linalg.eig(the_A - the_B @ K) max_eig = np.max(eigvals) # choose now kappa sucht that lam_max(A-BK) < - kappa kappa = 0.9 # # step 2: compute P from Lyapunov equation [n, p] = the_B.shape P = scipy.linalg.solve_continuous_lyapunov( the_A - the_B @ K + kappa * np.identity(n), -(the_Q + K.T @ the_R @ K)) # # step 3: min. x'Px # # s.t. u_min <= u <= u_max opti = casadi.Opti() opti.solver('ipopt') x = opti.variable(n) objective = x.T @ P @ x # shd be the same as -x.T@P@x, because we have equality constraint opti.minimize(objective) Aiq = np.concatenate((-np.identity(p), np.identity(p))) biq = np.concatenate((-the_umin, the_umax)) nm = np.size(biq) a = opti.parameter(p) b = opti.parameter() opti.subject_to(a.T @ K @ x == b) ALPHA = np.zeros(nm) for k in range(0, nm): opti.set_value(a, Aiq[k, :]) opti.set_value(b, biq[k]) sol = opti.solve() ALPHA[k] = sol.value(x) @ P @ sol.value(x) alpha = np.min(ALPHA) return P, alpha
def __init__(self): self.opti = ca.Opti() self.N = 200 self.d = 2.0 self.dmax = 20.0 self.umax = 20.0 self.pi = -np.pi self.l = 0.5 self.m1 = 0.5 self.m2 = 0.1 self.g = -9.81 self.T = 2.0 self.h = self.T / self.N goal1 = self.opti.parameter() goal2 = self.opti.parameter() goal3 = self.opti.parameter() goal4 = self.opti.parameter() self.opti.set_value(goal1, self.d) self.opti.set_value(goal2, self.pi) self.opti.set_value(goal3, 0) self.opti.set_value(goal4, 0) self.goal = [goal1, goal2, goal3, goal4] self.q1 = self.opti.variable(self.N) self.q2 = self.opti.variable(self.N) self.q1_dot = self.opti.variable(self.N) self.q2_dot = self.opti.variable(self.N) self.u = self.opti.variable(self.N) self.q1_ddot = ( ((self.l * self.m2 * ca.sin(self.q2) * (self.q2**2)) + (self.u) + (self.m2 * self.g * ca.cos(self.q2) * ca.sin(self.q2))) / (self.m1 + self.m2 * (ca.sin(self.q2)**2))) self.q2_ddot = ( ((self.l * self.m2 * ca.cos(self.q2) * ca.sin(self.q2) * (self.q2_dot**2)) + (self.u * ca.cos(self.q2)) + ((self.m1 + self.m2) * self.g * ca.sin(self.q2))) / (self.m1 * self.l + self.l * self.m2 * (ca.sin(self.q2)**2))) self.state = ca.horzcat(self.q1, self.q2, self.q1_dot, self.q2_dot) self.model = ca.horzcat(self.q1_dot, self.q2_dot, self.q1_ddot, self.q2_ddot)
def SingleStageOptimization(model,ref,N): """ single shooting procedure for optimal control of a scalar final value model: Quality Model ref: skalarer Referenzwert für Optimierungsproblem N: Anzahl an Zeitschritten """ # Create Instance of the Optimization Problem opti = cs.Opti() # Create decision variables for states U = opti.variable(N,1) # Initial quality x = 0 y = 0 X = [x] Y = [y] # Simulate Model for k in range(N): out = SimulateModel(model.ModelQuality,X[k],U[k],model.ModelParamsQuality) X.append(out[0]) Y.append(out[1]) X = hcat(X) Y = hcat(Y) # Define Loss Function opti.minimize(sumsqr(Y[-1]-ref)) #Choose solver opti.solver('ipopt') # Get solution sol = opti.solve() # Extract real values from solution values = {} values['U'] = sol.value(U) return values
def mpc(): opti = casadi.Opti() N = 10 #horizon length wu = 0.1 #weight of control effort (u) wx = 1 #weight of x's du_bounds = 0.15 r = 5 #setpoint xi = 0 xs = [] dus = [] for i in range(0, N): xs.append(opti.variable()) dus.append(opti.variable()) #Actuator effort constraints opti.subject_to(dus[-1] >= -du_bounds) opti.subject_to(dus[-1] <= du_bounds) #Dynamics constraint. Nessesary to have a different first constraint since the first x isn't a decsision variable #Dynamics are copied/pasted from the plant_dynamics_damaged_simple function since the casadi solver cannot have function calls in it if i == 0: opti.subject_to( xs[-1] - (1 * xi + 3 * dus[-1] + casadi.tanh(xi * 0.003) * 10 + dus[-1] * 5 * casadi.sin(xi * 0.2)) == 0) dus.append(opti.variable()) opti.subject_to(dus[-1] >= -du_bounds) opti.subject_to(dus[-1] <= du_bounds) else: opti.subject_to( xs[-1] - (1 * xs[-2] + 3 * dus[-2] + casadi.tanh(xs[-2] * 0.003) * 10 + dus[-2] * 5 * casadi.sin(xs[-2] * 0.2)) == 0) #Cost function for MPC taken from https://en.wikipedia.org/wiki/Model_predictive_control J = wx * (r - xi)**2 + wu * dus[0]**2 for i in range(1, N): J += wx * (r - xs[i - 1])**2 + wu * dus[i]**2 opti.minimize(J) opti.solver('ipopt') sol = opti.solve() print("xs " + str(xi) + " dus: " + str(sol.value(dus[0]))) for i in range(0, len(xs)): print("xs " + str(sol.value(xs[i])) + " dus: " + str(sol.value(dus[i + 1])))
def OPTController(self,e, tgt, C,maxCore): print(e,tgt,C) self.model = casadi.Opti() nApp=len(tgt) T=self.model.variable(1,nApp); S=self.model.variable(1,nApp); E_l1=self.model.variable(1,nApp); self.model.subject_to(T>=0) self.model.subject_to(self.model.bounded(0,S,maxCore)) self.model.subject_to(E_l1>=0) sSum=0 obj=0; for i in range(nApp): sSum+=S[0,i] #obj+=E_l1[0,i] obj+=(T[0,i]/C[i]-1/tgt[i])**2 self.model.subject_to(sSum<=maxCore) for i in range(nApp): # optCTRL.addCons(T[i] <= S[i] / e[i]) # optCTRL.addCons(T[i] <= C[i] / e[i]) # optCTRL.addCons(T[i] >= S[i] / e[i] - C[i] / e[i] * D[i]) # optCTRL.addCons(T[i] >= C[i] / e[i] - C[i] / e[i] * (1 - D[i])) # optCTRL.addCons(E_l1[i] >= ((C[i]/T[i])-tgt[i])) # optCTRL.addCons(E_l1[i] >= -((C[i]/T[i])-tgt[i])) self.model.subject_to(T[0,i]==casadi.fmin(S[0,i] / e[i],C[i] / e[i])) # self.model.subject_to((E_l1[0,i]+tgt[i])>=((C[i]/T[0,i]))) # self.model.subject_to((E_l1[0,i]-tgt[i])>=-((C[i]/T[0,i]))) self.model.minimize(obj) optionsIPOPT={'print_time':False,'ipopt':{'print_level':0}} #self.model.solver('osqp',{'print_time':False,'error_on_fail':False}) self.model.solver('ipopt',optionsIPOPT) sol=self.model.solve() return sol.value(S).tolist()
def test_dh_matrix_casadi(self): dh_params = DHLink(0.1, np.pi / 4, -0.1, np.pi / 6) link1 = LinkKinematics(dh_params, JointType.revolute) link2 = LinkKinematics(dh_params, JointType.prismatic) opti = ca.Opti() q1 = opti.variable() T1 = ca.Function("T1", [q1], [link1.get_link_relative_transform_casadi(q1)]) T1_desired = DenHarMat(1.2, dh_params.alpha, dh_params.a, dh_params.d) assert_almost_equal(np.array(T1(1.2)), T1_desired) d1 = opti.variable() T2 = ca.Function("T2", [d1], [link2.get_link_relative_transform_casadi(d1)]) T2_desired = DenHarMat(dh_params.theta, dh_params.alpha, dh_params.a, 0.75) assert_almost_equal(np.array(T2(0.75)), T2_desired)
def __init__( self, the_A, # discrete system matrix (should be scipy.sparse) the_B, # discrete input matrix (should be scipy.sparse) the_N, # discrete horizon the_Q, # state penalty (should be scipy.sparse) the_R, # input penalty (should be scipy.sparse) the_P, # defines terminal region the_alpha, # defines terminal region the_x_min, # lower bound on x (should be np.array) use -np.inf/np.inf if unconstraint the_x_max, # upper bound on x the_u_min, # lower bounf on u the_u_max # upper bound on u ): self.__N = the_N self.__A = the_A self.__B = the_B self.__Q = the_Q self.__R = the_R self.__P = the_P self.__alpha = the_alpha self.__xmin = the_x_min self.__xmax = the_x_max self.__umin = the_u_min self.__umax = the_u_max [self.__n, self.__p] = the_B.shape # state and input dimension # prealocate logging array self.predictedStateTrajectory = np.zeros((self.__n, self.__N + 1)) self.predictedInputTrajectory = np.zeros((self.__p, self.__N)) # define optimization variables self.__opti = casadi.Opti() self.__U = self.__opti.variable(self.__p, self.__N) self.__X = self.__opti.variable(self.__n, self.__N + 1) self.__IC = self.__opti.parameter(self.__n) # build problem self.__buildProblem()
def control(self, t, delta_t): c_opti = casadi.Opti() self.c_opti = c_opti u = c_opti.variable(self.m) ## likely this issue, can prob easily be fixed component wise thing # NEWNEW first remove umax constraint c_opti.subject_to(u**2 <= self.umax**2) # check to make sure new move is within bounds x_pred = self.move(u, t, delta_t, is_pred=True) c_opti.subject_to(x_pred >= 0) c_opti.subject_to(x_pred <= np.array(self.U_shape)) c_opti.set_initial(u, self.u_log[-1]) c_k_pred, _ = self.recalculate_c_k(t, delta_t, x_pred=x_pred) c_opti.minimize(calculate_ergodicity(self.k_bands, lambd, c_k_pred, mu)) p_opts = {} s_opts = {'print_level': 0} c_opti.solver('ipopt', p_opts, s_opts) sol = c_opti.solve() return sol.value(u)
def control_casadi(self, t, delta_t): c_opti = casadi.Opti() self.c_opti = c_opti u = c_opti.variable(self.m) c_opti.subject_to(u <= self.umax) c_opti.subject_to(u >= -self.umax) # check to make sure new move is within bounds x_pred = self.move(u, t, delta_t, is_pred=True) c_opti.subject_to(x_pred >= 0) c_opti.subject_to(x_pred <= np.array(self.U_shape)) c_opti.set_initial(u, self.u_log[-1]) c_k_pred, _ = self.recalculate_c_k(t, delta_t, x_pred=x_pred) c_opti.minimize(calculate_ergodicity(self.k_bands, lambd, c_k_pred, mu)) p_opts = {} s_opts = {'print_level': 0} c_opti.solver('ipopt', p_opts, s_opts) sol = c_opti.solve() return sol.value(u)