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)
