How to use the casadi.Function function in casadi
To help you get started, we’ve selected a few casadi examples, based on popular ways it is used in public projects.
Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
helgeanl / GP-MPC / gp_casadi / mpc.py View on Github 
elif method is 'TA':
gp_func = ca.Function('gp_taylor_approx', [z_s, covar_x_s],
gp_taylor_approx(invK, ca.MX(X), ca.MX(Y),
ca.MX(hyper), z_s.T, covar_x_s,
meanFunc=meanFunc, diag=True, log=log))
elif method is 'EM':
gp_func = ca.Function('gp_taylor_approx', [z_s, covar_x_s],
gp_taylor_approx(invK, ca.MX(X), ca.MX(Y),
ca.MX(hyper), z_s.T, covar_x_s,
diag=True))
else:
raise NameError('No GP method called: ' + method)
# Define stage cost and terminal cost
if costFunc is 'quad':
l_func = ca.Function('l', [mean_s, covar_x_s, u_s, delta_u_s, K_s],
[cost_l(mean_s, ca.MX(mean_ref), covar_x_s, u_s, delta_u_s,
ca.MX(Q), ca.MX(R_u), ca.MX(R_du), K_s)])
lf_func = ca.Function('lf', [mean_s, covar_x_s],
[cost_lf(mean_s, ca.MX(mean_ref), covar_x_s, ca.MX(P))])
elif costFunc is 'sat':
l_func = ca.Function('l', [mean_s, covar_x_s, u_s, delta_u_s, K_s],
[cost_saturation_l(mean_s, ca.MX(mean_ref), covar_x_s, u_s, delta_u_s,
ca.MX(Q), ca.MX(R_u), ca.MX(R_du), K_s)])
lf_func = ca.Function('lf', [mean_s, covar_x_s],
[cost_saturation_lf(mean_s, ca.MX(mean_ref), covar_x_s, ca.MX(P))])
else:
raise NameError('No cost function called: ' + costFunc)
# Feedback function
if feedback:
u_func = ca.Function('u', [mean_s, v_s, K_s],# Concatenate vectors
w = ca.vertcat(*w)
g = ca.vertcat(*g)
x_plot = ca.horzcat(*x_plot)
u_plot = ca.horzcat(*u_plot)
w0 = np.concatenate(w0)
lbw = np.concatenate(lbw)
ubw = np.concatenate(ubw)
lbg = np.concatenate(lbg)
ubg = np.concatenate(ubg)
# Create an NLP solver
prob = {'f': J, 'x': w, 'g': g, 'p': P}
# Function to get x and u trajectories from w
trajectories = ca.Function('trajectories', [w], [x_plot, u_plot], ['w'], ['x', 'u'])
# Create an NLP solver, using SQP and active-set QP for accurate multipliers
opts = dict(qpsol='qrqp', qpsol_options=dict(print_iter=False,error_on_fail=False), print_time=False)
solver = ca.nlpsol('solver', 'sqpmethod', prob, opts)
# Solve the NLP
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg, p=0)
# Extract trajectories
x_opt, u_opt = trajectories(sol['x'])
x_opt = x_opt.full() # to numpy array
u_opt = u_opt.full() # to numpy array
# Plot the result
tgrid = np.linspace(0, T, N+1)
plt.figure(1)'linear': m(x) = aT*x + b
'polynomial': m(x) = aT*x^2 + bT*x + c
"""
Nx, N = X.shape
X_s = ca.SX.sym('x', Nx, N)
Z_s = ca.MX.sym('x', Nx, N)
m = ca.SX(N, 1)
hyp_s = ca.SX.sym('hyper', *hyper.shape)
if func == 'zero':
meanF = ca.Function('zero_mean', [X_s, hyp_s], [m])
elif func == 'const':
a = hyp_s[-1]
for i in range(N):
m[i] = a
meanF = ca.Function('const_mean', [X_s, hyp_s], [m])
elif func == 'linear':
a = hyp_s[-Nx-1:-1].reshape((1, Nx))
b = hyp_s[-1]
for i in range(N):
m[i] = ca.mtimes(a, X_s[:,i]) + b
meanF = ca.Function('linear_mean', [X_s, hyp_s], [m])
elif func == 'polynomial':
a = hyp_s[-2*Nx-1:-Nx-1].reshape((1,Nx))
b = hyp_s[-Nx-1:-1].reshape((1,Nx))
c = hyp_s[-1]
for i in range(N):
m[i] = ca.mtimes(a, X_s[:, i]**2) + ca.mtimes(b, X_s[:,i]) + c
meanF = ca.Function('poly_mean', [X_s, hyp_s], [m])
else:
raise NameError('No mean function called: ' + func)
def sx_function(name, inputs, outputs):
return ca.Function(name, inputs, outputs)# Cost function
Q = diag([1.0, 1.0])
R = 1e-2
x = SX.sym('x', nx)
u = SX.sym('u', nu)
u_N = SX.sym('u', 0)
f = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)]))
f_N = ocp_nlp_function(Function('ls_cost_N', [x, u_N], [x]))
ls_cost = ocp_nlp_ls_cost(N, N*[f]+[f_N])
ls_cost.ls_cost_matrix = N*[block_diag(Q, R)] + [Q]
nlp.set_cost(ls_cost)
# Constraints
g = ocp_nlp_function(Function('path_constraint', [x, u], [u]))
g_N = ocp_nlp_function(Function('path_constraintN', [x, u], [SX([])]))
nlp.set_path_constraints(N*[g] + [g_N])
for i in range(N):
nlp.lg[i] = -0.5
nlp.ug[i] = +0.5
solver = ocp_nlp_solver('sqp', nlp, {'sim_solver': 'lifted-irk', 'integrator_steps': 2, 'qp_solver':'hpipm', 'sensitivity_method': 'gauss-newton'})
# Simulation
STATES = [array([0.1, 0.1])]
CONTROLS = []
for i in range(20):
state_traj, control_traj = solver.evaluate(STATES[-1])
STATES += [state_traj[1]]
CONTROLS += [control_traj[0]]
plt.ion()def get_mean_function(hyper, X, func='zero'):
""" Get mean function
'zero': m = 0
'const': m = a
'linear': m(x) = aT*x + b
'polynomial': m(x) = aT*x^2 + bT*x + c
"""
Nx, N = X.shape
X_s = ca.SX.sym('x', Nx, N)
Z_s = ca.MX.sym('x', Nx, N)
m = ca.SX(N, 1)
hyp_s = ca.SX.sym('hyper', *hyper.shape)
if func == 'zero':
meanF = ca.Function('zero_mean', [X_s, hyp_s], [m])
elif func == 'const':
a = hyp_s[-1]
for i in range(N):
m[i] = a
meanF = ca.Function('const_mean', [X_s, hyp_s], [m])
elif func == 'linear':
a = hyp_s[-Nx-1:-1].reshape((1, Nx))
b = hyp_s[-1]
for i in range(N):
m[i] = ca.mtimes(a, X_s[:,i]) + b
meanF = ca.Function('linear_mean', [X_s, hyp_s], [m])
elif func == 'polynomial':
a = hyp_s[-2*Nx-1:-Nx-1].reshape((1,Nx))
b = hyp_s[-Nx-1:-1].reshape((1,Nx))
c = hyp_s[-1]
for i in range(N):else:
fun_name = 'expl_vde_for'
Sx = casadi.SX.sym('Sx', nx, nx)
Su = casadi.SX.sym('Su', nx, nu)
vde_x = casadi.jtimes(fun, x, Sx)
vde_u = casadi.jacobian(fun, u) + casadi.jtimes(fun, x, Su)
casadi_fun = casadi.Function(fun_name, [x, Sx, Su, u], [fun, vde_x, vde_u])
casadi_fun.generate(casadi_opts)
c_sources = c_sources + fun_name + '.c '
elif opts.scheme=='irk':
fun_name = 'impl_ode_fun'
casadi_fun = casadi.Function(fun_name, [x, xdot, u, z], [fun])
casadi_fun.generate(casadi_opts)
c_sources = c_sources + fun_name + '.c '
fun_name = 'impl_ode_fun_jac_x_xdot_z'
jac_x = casadi.jacobian(fun, x)
jac_xdot = casadi.jacobian(fun, xdot)
jac_z = casadi.jacobian(fun, z)
casadi_fun = casadi.Function(fun_name, [x, xdot, u, z], [fun, jac_x, jac_xdot, jac_z])
casadi_fun.generate(casadi_opts)
c_sources = c_sources + fun_name + '.c '
if opts.sens_forw=='true':
fun_name = 'impl_ode_jac_x_xdot_u_z'
jac_x = casadi.jacobian(fun, x)
jac_xdot = casadi.jacobian(fun, xdot)xi_max = 0.5 * np.pi / xi_s # max. relative angle to tangent on reference line [rad]
# ------------------------------------------------------------------------------------------------------------------
# INITIAL GUESS FOR DECISION VARIABLES -----------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
v_guess = 20.0 / v_s
# ------------------------------------------------------------------------------------------------------------------
# HELPER FUNCTIONS -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# continuous time dynamics
f_dyn = ca.Function('f_dyn', [x, u, kappa], [dx, sf], ['x', 'u', 'kappa'], ['dx', 'sf'])
# longitudinal tire forces [N]
f_fx = ca.Function('f_fx', [x, u], [f_x_fl, f_x_fr, f_x_rl, f_x_rr],
['x', 'u'], ['f_x_fl', 'f_x_fr', 'f_x_rl', 'f_x_rr'])
# lateral tire forces [N]
f_fy = ca.Function('f_fy', [x, u], [f_y_fl, f_y_fr, f_y_rl, f_y_rr],
['x', 'u'], ['f_y_fl', 'f_y_fr', 'f_y_rl', 'f_y_rr'])
# vertical tire forces [N]
f_fz = ca.Function('f_fz', [x, u], [f_z_fl, f_z_fr, f_z_rl, f_z_rr],
['x', 'u'], ['f_z_fl', 'f_z_fr', 'f_z_rl', 'f_z_rr'])
# longitudinal and lateral acceleration [m/s²]
f_a = ca.Function('f_a', [x, u], [ax, ay], ['x', 'u'], ['ax', 'ay'])
if pars["pwr_params_mintime"]["pwr_behavior"]:
machine.ini_nlp_state(x=x, u=u)
inverter.ini_nlp_state(x=x, u=u)
batt.ini_nlp_state(x=x, u=u)# Create symbols
Q_s = ca.SX.sym('Q', Nx, Nx)
R_s = ca.SX.sym('Q', Nu, Nu)
x_s = ca.SX.sym('x', Nx)
u_s = ca.SX.sym('x', Nu)
covar_x_s = ca.SX.sym('covar_z', Nx, Nx)
covar_u_s = ca.SX.sym('covar_u', ca.MX.size(R))
Z_x = ca.SX.eye(Nx) + 2 * covar_x_s @ Q_s
Z_u = ca.SX.eye(Nu) + 2 * covar_u_s @ R_s
cost_x = ca.Function('cost_x', [x_s, Q_s, covar_x_s],
[1 - ca.exp(-(x_s.T @ ca.solve(Z_x.T, Q_s.T).T @ x_s))
/ ca.sqrt(ca.det(Z_x))])
cost_u = ca.Function('cost_u', [u_s, R_s, covar_u_s],
[1 - ca.exp(-(u_s.T @ ca.solve(Z_u.T, R_s.T).T @ u_s))
/ ca.sqrt(ca.det(Z_u))])
return cost_x(x - x_ref, Q, covar_x) + cost_u(u, R, covar_u)expr = self.get_mx(tree)
# Obtain the symbols it depends on
free_vars = ca.symvar(expr)
# Find the values of the symbols
vals = []
for free_var in free_vars:
if free_var.is_symbolic():
if (len(self.for_loops) > 0) and (free_var.name() == self.for_loops[-1].name):
vals.append(self.for_loops[-1].index_variable)
else:
vals.append(self.get_integer(self.current_class.symbols[free_var.name()].value))
# Evaluate the expression
F = ca.Function('get_integer', free_vars, [expr])
ret = F.call(vals, *self.function_mode)
if ret[0].is_constant():
# We managed to evaluate the expression. Assume the result to be integer.
return int(ret[0])
else:
# Expression depends on other symbols. Could not extract integer value.
return ret[0]
if isinstance(tree, ast.Slice):
start = self.get_integer(tree.start)
step = self.get_integer(tree.step)
stop = self.get_integer(tree.stop)
return slice(start, stop, step)
else:
raise Exception('Unexpected node type {}'.format(tree.__class__.__name__))casadi
CasADi -- framework for algorithmic differentiation and numeric optimization
