def get_dependency(expression):
sym = symvar(expression)
f = Function('f', sym, [expression])
dep = col.OrderedDict()
for index, sym in enumerate(sym):
J = f.sparsity_jac(index, 0)
dep[sym] = sorted(set(J.T.row()))
return dep
def _codegen_model(model_folder: str, f: ca.Function, library_name: str):
# Compile shared libraries
if os.name == 'posix':
compiler_flags = ['-O2', '-fPIC']
linker_flags = ['-fPIC']
else:
compiler_flags = ['/O2', '/wd4101'] # Shut up unused local variable warnings.
linker_flags = ['/DLL']
# Generate C code
logger.debug("Generating {}".format(library_name))
cg = ca.CodeGenerator(library_name)
cg.add(f, True) # Nondifferentiated function
cg.add(f.forward(1), True) # Jacobian-times-vector product
cg.add(f.reverse(1), True) # vector-times-Jacobian product
cg.add(f.reverse(1).forward(1), True) # Hessian-times-vector product
cg.generate(model_folder + '/')
compiler = distutils.ccompiler.new_compiler()
file_name = os.path.relpath(os.path.join(model_folder, library_name + '.c'))
object_name = compiler.object_filenames([file_name])[0]
library = os.path.join(model_folder, library_name + compiler.shared_lib_extension)
try:
# NOTE: For some reason running in debug mode in PyCharm (2017.1)
# on Windows causes cl.exe to fail on its own binary name (?!) and
# the include paths. This does not happen when running directly
# from cmd.exe / PowerShell or e.g. with debug mode in VS Code.
compiler.compile([file_name], extra_postargs=compiler_flags)
# We do not want the "lib" prefix on POSIX systems, so we call
# link() directly with our desired filename instead of
# link_shared_lib().
compiler.link(compiler.SHARED_LIBRARY, [object_name], library, extra_preargs=linker_flags)
except:
raise
finally:
with contextlib.suppress(FileNotFoundError):
os.remove(file_name)
os.remove(object_name)
return library
raise ValueError("string_to_test should be: 'G', 'D', 'A' or 'E'")
m = biorbd.Model(model_path)
bound_min = []
bound_max = []
for i in range(m.nbSegment()):
seg = m.segment(i)
for r in seg.QRanges():
bound_min.append(r.min())
bound_max.append(r.max())
bounds = (bound_min, bound_max)
pn = DummyPenalty(m)
ObjectiveFcn.Lagrange.TRACK_SEGMENT_WITH_CUSTOM_RT.value[0](
pn, pn, idx_segment_bow_hair, rt_on_string)
custom_rt = Function("custom_rt", [pn.nlp.q], [pn.val]).expand()
ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS.value[0](pn, pn, tag_bow_contact,
tag_violin)
superimpose = Function("superimpose", [pn.nlp.q], [pn.val]).expand()
def objective_function(x, *args, **kwargs):
out = np.ndarray((6, ))
out[:3] = np.array(custom_rt(x))[:, 0]
out[3:] = np.array(superimpose(x))[:, 0]
return out
b = bioviz.Viz(loaded_model=m,
markers_size=0.003,
show_markers=True,
def __set_costs(self, ocp):
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
# set Lagrange terms
self.acados_ocp.cost.Vx = np.zeros(
(self.acados_ocp.dims.ny, self.acados_ocp.dims.nx))
self.acados_ocp.cost.Vx[:self.acados_ocp.dims.nx, :] = np.eye(
self.acados_ocp.dims.nx)
Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu))
Vu[self.acados_ocp.dims.nx:, :] = np.eye(self.acados_ocp.dims.nu)
self.acados_ocp.cost.Vu = Vu
# set Mayer term
self.acados_ocp.cost.Vx_e = np.zeros(
(self.acados_ocp.dims.nx, self.acados_ocp.dims.nx))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
if ocp.nb_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet")
for i in range(ocp.nb_phases):
# TODO: I think ocp.J is missing here (the parameters would be stored there)
# TODO: Yes the objectives in ocp are not dealt with,
# does that makes sense since we only work with 1 phase ?
for j, J in enumerate(ocp.nlp[i]["J"]):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
if J[0]["target"] is not None:
self.y_ref_end.append([J[0]["target"]])
else:
self.y_ref_end.append(
[np.zeros((J[0]["val"].numel(), 1))])
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.lagrange_costs.numel():
self.acados_ocp.model.cost_y_expr = self.lagrange_costs
else:
self.acados_ocp.model.cost_y_expr = SX(1, 1)
if self.mayer_costs.numel():
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs
else:
self.acados_ocp.model.cost_y_expr_e = SX(1, 1)
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny,
1), ))
self.acados_ocp.cost.yref_e = np.concatenate(self.y_ref_end,
-1).T.squeeze()
if self.W.shape == (0, 0):
self.acados_ocp.cost.W = np.zeros((1, 1))
else:
self.acados_ocp.cost.W = self.W
if self.W_e.shape == (0, 0):
self.acados_ocp.cost.W_e = np.zeros((1, 1))
else:
self.acados_ocp.cost.W_e = self.W_e
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
for i in range(ocp.nb_phases):
for j in range(len(ocp.nlp[i]["J"])):
J = ocp.nlp[i]["J"][j][0]
raise RuntimeError(
"TODO: The target is not right currently")
if J["type"] == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J["val"][0] - J["target"][0])
elif J["type"] == ObjectiveFunction.MayerFunction:
raise RuntimeError(
"TODO: I may have broken this (is this the right J?)"
)
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J["val"]])
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
self.acados_ocp.model.cost_expr_ext_cost = sum1(
self.lagrange_costs)
self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs)
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def markers_fun(biorbd_model):
qMX = MX.sym('qMX', biorbd_model.nbQ())
return Function('markers', [qMX], [biorbd_model.markers(qMX)])
def configure_soft_contact_function(ocp, nlp):
"""
Configure the soft contact sphere
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
nlp: NonLinearProgram
A reference to the phase
"""
global_soft_contact_force_func = MX.zeros(
nlp.model.nbSoftContacts() * 6, 1)
n = nlp.model.nbQ()
component_list = ["Mx", "My", "Mz", "Fx", "Fy", "Fz"]
for i_sc in range(nlp.model.nbSoftContacts()):
soft_contact = nlp.model.softContact(i_sc)
global_soft_contact_force_func[i_sc * 6:(i_sc + 1) * 6, :] = (
biorbd.SoftContactSphere(soft_contact).computeForceAtOrigin(
nlp.model, nlp.states.mx_reduced[:n],
nlp.states.mx_reduced[n:]).to_mx())
nlp.soft_contact_forces_func = Function(
"soft_contact_forces_func",
[
nlp.states.mx_reduced, nlp.controls.mx_reduced,
nlp.parameters.mx
],
[global_soft_contact_force_func],
["x", "u", "p"],
["soft_contact_forces"],
).expand()
for i_sc in range(nlp.model.nbSoftContacts()):
all_soft_contact_names = []
all_soft_contact_names.extend([
f"{nlp.model.softContactName(i_sc).to_string()}_{name}"
for name in component_list if nlp.model.softContactName(
i_sc).to_string() not in all_soft_contact_names
])
if "soft_contact_forces" in nlp.plot_mapping:
phase_mappings = nlp.plot_mapping["soft_contact_forces"]
else:
soft_contact_names_in_phase = [
f"{nlp.model.softContactName(i_sc).to_string()}_{name}"
for name in component_list if nlp.model.softContactName(
i_sc).to_string() not in all_soft_contact_names
]
phase_mappings = Mapping([
i for i, c in enumerate(all_soft_contact_names)
if c in soft_contact_names_in_phase
])
nlp.plot[
f"soft_contact_forces_{nlp.model.softContactName(i_sc).to_string()}"] = CustomPlot(
lambda t, x, u, p: nlp.soft_contact_forces_func(x, u, p)[
(i_sc * 6):((i_sc + 1) * 6), :],
plot_type=PlotType.INTEGRATED,
axes_idx=phase_mappings,
legend=all_soft_contact_names,
)
A_d = SX([[1, 0.1, 0], [0, -0.14957, 0.024395], [0, 0, 0.82456]])
B_d = SX([[0], [0], [0.054386]])
T = 0.1
h = SX.sym('h')
h_p = SX.sym('h_p')
eta = SX.sym('eta')
states = vertcat(h, h_p, eta)
u_pwm = SX.sym('u_pwm')
controls = vertcat(u_pwm)
rhs = vertcat(h_p,
k_L / m * ((k_V * (eta + eta_0) - A_B * h_p) / A_SP)**2 - g,
-1 / T_M * eta + k_M / T_M * u_pwm)
f = Function('f', [states, controls], [rhs * T + states])
# K = SX([[0.99581, 0.086879, 0.1499]])
K = SX([[31.2043, 2.7128, 0.4824]])
phi_c = A_d - B_d @ K
P_f = SX([[5959.1, 517.92, 65.058], [517.92, 51.198, 5.6392],
[65.058, 5.6392, 641.7]])
# P_f=SX([[2.4944e+005,20941,1932.4],[20941,1839.4,168.05],[1932.4,168.05,641.7]])
state_r = SX([[0.8], [0], [48.760258862]])
u_r = [157.291157619]
Q = SX.zeros(3, 3)
# Q[0, 0] = 1e4
Q[0, 0] = 1
Q[1, 1] = 1
def generate_data(biorbd_model, final_time, nb_shooting):
# Aliases
nb_q = biorbd_model.nbQ()
nb_qdot = biorbd_model.nbQdot()
nb_tau = biorbd_model.nbGeneralizedTorque()
nb_mus = biorbd_model.nbMuscleTotal()
nb_markers = biorbd_model.nbMarkers()
dt = final_time / nb_shooting
# Casadi related stuff
symbolic_states = MX.sym("x", nb_q + nb_qdot + nb_mus, 1)
symbolic_controls = MX.sym("u", nb_tau + nb_mus, 1)
nlp = {
"model":
biorbd_model,
"nbTau":
nb_tau,
"nbQ":
nb_q,
"nbQdot":
nb_qdot,
"nbMuscle":
nb_mus,
"q_mapping":
BidirectionalMapping(Mapping(range(nb_q)), Mapping(range(nb_q))),
"q_dot_mapping":
BidirectionalMapping(Mapping(range(nb_qdot)), Mapping(range(nb_qdot))),
"tau_mapping":
BidirectionalMapping(Mapping(range(nb_tau)), Mapping(range(nb_tau))),
}
markers_func = []
for i in range(nb_markers):
markers_func.append(
Function(
"ForwardKin",
[symbolic_states],
[biorbd_model.marker(symbolic_states[:nb_q], i).to_mx()],
["q"],
["marker_" + str(i)],
).expand())
dynamics_func = Function(
"ForwardDyn",
[symbolic_states, symbolic_controls],
[
Dynamics.forward_dynamics_muscle_excitations_and_torque_driven(
symbolic_states, symbolic_controls, nlp)
],
["x", "u"],
["xdot"],
).expand()
def dyn_interface(t, x, u):
u = np.concatenate([np.zeros(2), u])
return np.array(dynamics_func(x, u)).squeeze()
# Generate some muscle excitations
U = np.random.rand(nb_shooting, nb_mus)
# Integrate and collect the position of the markers accordingly
X = np.ndarray(
(biorbd_model.nbQ() + biorbd_model.nbQdot() + nb_mus, nb_shooting + 1))
markers = np.ndarray((3, biorbd_model.nbMarkers(), nb_shooting + 1))
def add_to_data(i, q):
X[:, i] = q
for j, mark_func in enumerate(markers_func):
markers[:, j, i] = np.array(mark_func(q)).squeeze()
x_init = np.array([0] * nb_q + [0] * nb_qdot + [0.5] * nb_mus)
add_to_data(0, x_init)
for i, u in enumerate(U):
sol = solve_ivp(dyn_interface, (0, dt),
x_init,
method="RK45",
args=(u, ))
x_init = sol["y"][:, -1]
add_to_data(i + 1, x_init)
time_interp = np.linspace(0, final_time, nb_shooting + 1)
return time_interp, markers, X, U
def generate_c_code_external_cost(model, stage_type):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double")
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
x = model.x
p = model.p
if isinstance(x, MX):
symbol = MX.sym
else:
symbol = SX.sym
if stage_type == 'terminal':
suffix_name = "_cost_ext_cost_e_fun"
suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_e_fun_jac"
u = symbol("u", 0, 0)
ext_cost = model.cost_expr_ext_cost_e
elif stage_type == 'path':
suffix_name = "_cost_ext_cost_fun"
suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost
elif stage_type == 'initial':
suffix_name = "_cost_ext_cost_0_fun"
suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_0_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost_0
# set up functions to be exported
fun_name = model.name + suffix_name
fun_name_hess = model.name + suffix_name_hess
fun_name_jac = model.name + suffix_name_jac
# generate expression for full gradient and Hessian
full_hess, grad = hessian(ext_cost, vertcat(u, x))
ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost])
ext_cost_fun_jac_hess = Function(fun_name_hess, [x, u, p],
[ext_cost, grad, full_hess])
ext_cost_fun_jac = Function(fun_name_jac, [x, u, p], [ext_cost, grad])
# generate C code
if not os.path.exists("c_generated_code"):
os.mkdir("c_generated_code")
os.chdir("c_generated_code")
gen_dir = model.name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = "./" + gen_dir
os.chdir(gen_dir_location)
ext_cost_fun.generate(fun_name, casadi_opts)
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts)
ext_cost_fun_jac.generate(fun_name_jac, casadi_opts)
os.chdir("../..")
return
def objective(x,y,p,N,params):
nPrimal = x.numel() #number of primal sln
nDual = y['lam_g'].numel() #number of dual
nParam = p.numel() #number of parameters
#Loading initial states and controls
data = spio.loadmat('CstrDistXinit.mat', squeeze_me = True)
Xinit = data['Xinit']
xf = Xinit[0:84]
u_opt = Xinit[84:]
#Model parameters
NT = params['dist']['NT'] #Stages in column
Uf = 0.3 #Feed rate to CSTR F_0
_,state,xdot,inputs = ColCSTR_model(Uf,params)
sf = Function('sf',[state,inputs],[xdot])
params['model']['sf'] = sf
params['model']['xdot_val_rf_ss'] = xf
params['model']['x'] = x
params['model']['u_opt'] = u_opt
nx = params['prob']['nx']
nu = params['prob']['nu']
nk = params['prob']['nk']
tf = params['prob']['tf']
ns = params['prob']['ns']
h = params['prob']['h']
#Preparing collocation matrices
_, C, D, d = collocationSetup()
params['prob']['d'] = d
colloc = {'C':C,'D':D, 'h':h}
params['colloc'] = colloc
#NLP variable vector
V = MX() #Decision variables (control + state)
obj = 0 #Objective function
cons = MX() #Nonlinear Constraints
delta_time = 1
alpha = 1
beta = 1
gamma = 1
params['weight']['delta_time'] = delta_time
params['weight']['alpha'] = alpha
params['weight']['beta'] = beta
params['weight']['gamma'] = gamma
#Initial states and Controls
data_init = spio.loadmat('CstrDistXinit.mat', squeeze_me = True)
xf = data_init['Xinit'][0:84]
u_opt = data_init['Xinit'][84:89]
#"Lift" Initial conditions
X0 = MX.sym('X0', nx)
V = vertcat(V,X0) #Decision variables
cons = vertcat(cons, X0 - x[0:nx,0]) #Nonlinear constraints
cons_x0 = X0 - x[0:nx,0]
#Formulating the NLP
Xk = X0
data = spio.loadmat('Qmax.mat', squeeze_me = True)
params['Qmax'] = data['Qmax']
ssoftc = 0
for i in range(0,N):
obj, cons, V, Xk, params, ssoftc = itPredHorizon_pf(Xk, V, cons, obj, params, i, ssoftc)
V = vertcat(V[:])
cons = vertcat(cons[:])
#Objective function and constraint functions
f = Function('f', [V], [obj], ['V'], ['objective'])
c = Function('c', [V], [cons], ['V'], ['constraint'])
cx0 = Function('cx0',[X0], [cons_x0], ['X0'], ['constraint'])
#Constructing Lagrangian
lag_expr = obj + mtimes(transpose(y['lam_g']),cons)
g = Function('g',[V],[jacobian(obj,V),obj])
lagr = Function('lagr', [V], [lag_expr], ['V'], ['lag_expr'])
[H,gg] = hessian(lag_expr,V)
H = Function('H',[V],[H,gg])
[Hobj,gobj] = hessian(obj,V)
Hobj = Function('Hobj', [V], [Hobj,gobj])
J = Function('J',[V],[jacobian(cons,V),cons])
Jp = Function('Jp',[X0],[jacobian(cons_x0,X0),cons_x0])
#Evaluating functions at current point
f = f(x)
g = g(x)
g = g[0]
g = transpose(g)
H = H(x)
H = H[0]
Lxp = H[0:nPrimal,0:nParam]
J = J(x)
J = J[0]
Jtemp = DM.zeros((nDual,nParam))
cp = Jp(x[0:nParam])
cp = cp[0]
Jtemp[0:nParam, 0:nParam] = cp.full()
cp = Jtemp.sparse()
cst = c(x)
#Evaluation of objective function used for Greshgorin bound
Hobj = Hobj(x)
Hobj = Hobj[0].sparse()
f = f.full()
g = g.sparse()
Lxp = Lxp.sparse()
cst = cst.full()
#Equality constraint
Jeq = J
dpe = cp
return f, g, H, Lxp, cst, J, cp, Jeq, dpe, Hobj
N = 10
ode_fun, nx, nu = chen_model(implicit=True)
nlp = ocp_nlp({'N': N, 'nx': nx, 'nu': nu, 'ng': N * [1] + [0]})
# ODE Model
step = 0.1
nlp.set_model(ode_fun, step)
# 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, {
def gp_pred_function(x,
hyp,
kern_types,
x_train=None,
beta=None,
k_inv_training=None,
pred_var=True,
compute_grads=False):
"""
"""
n_gps = len(kern_types)
inp = SX.sym("input", x.shape)
out_dict = dict()
mu_all = []
pred_sigma_all = []
jac_mu_all = []
for i in range(n_gps):
kern_i = _get_kernel_function(kern_types[i], hyp[i])
if beta is None:
beta_i = None
else:
beta_i = beta[:, i]
k_inv_i = None
if not k_inv_training is None:
k_inv_i = k_inv_training[i]
if pred_var:
mu_new, sigma_new = gp_pred(inp, kern_i, beta_i, x_train, k_inv_i,
pred_var)
pred_func = Function("pred_func", [inp], [mu_new, sigma_new],
["inp"], ["mu_1", "sigma_1"])
F_1 = pred_func(inp=x)
pred_sigma = F_1["sigma_1"]
pred_sigma_all = horzcat(pred_sigma_all, pred_sigma)
else:
mu_new = gp_pred(inp, kern_i, beta_i, x_train, k_inv_i, pred_var)
pred_func = Function("pred_func", [inp], [mu_new], ["inp"],
["mu_1"])
F_1 = pred_func(inp=x)
mu_1 = F_1["mu_1"]
mu_all = horzcat(mu_all, mu_1)
if compute_grads:
# jac_func = pred_func.jacobian("inp","mu_1")
jac_func = pred_func.factory('dmudinp', ['inp'], ['jac:mu_1:inp'])
F_1_jac = jac_func(inp=x)
# print(F_1_jac)
jac_mu = F_1_jac['jac_mu_1_inp']
jac_mu_all = vertcat(jac_mu_all, jac_mu)
out_dict["pred_mu"] = mu_all
if pred_var:
out_dict["pred_sigma"] = pred_sigma_all
if compute_grads:
out_dict["jac_mu"] = jac_mu_all
return out_dict
def dxdt(self, h: float, states: Union[MX, SX], controls: Union[MX, SX],
params: Union[MX, SX]) -> tuple:
"""
The dynamics of the system
Parameters
----------
h: float
The time step
states: Union[MX, SX]
The states of the system
controls: Union[MX, SX]
The controls of the system
params: Union[MX, SX]
The parameters of the system
Returns
-------
The derivative of the states
"""
nu = controls.shape[0]
nx = states.shape[0]
# Choose collocation points
time_points = [0] + collocation_points(self.degree, "legendre")
# Coefficients of the collocation equation
C = self.CX.zeros((self.degree + 1, self.degree + 1))
# Coefficients of the continuity equation
D = self.CX.zeros(self.degree + 1)
# Dimensionless time inside one control interval
time_control_interval = self.CX.sym("time_control_interval")
# For all collocation points
for j in range(self.degree + 1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
L = 1
for r in range(self.degree + 1):
if r != j:
L *= (time_control_interval -
time_points[r]) / (time_points[j] - time_points[r])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
lfcn = Function("lfcn", [time_control_interval], [L])
D[j] = lfcn(1.0)
# Evaluate the time derivative of the polynomial at all collocation points to get
# the coefficients of the continuity equation
tfcn = Function("tfcn", [time_control_interval],
[tangent(L, time_control_interval)])
for r in range(self.degree + 1):
C[j, r] = tfcn(time_points[r])
# Total number of variables for one finite element
x0 = states
u = controls
x_irk_points = [
self.CX.sym(f"X_irk_{j}", nx, 1)
for j in range(1, self.degree + 1)
]
x = [x0] + x_irk_points
x_irk_points_eq = []
for j in range(1, self.degree + 1):
t_norm_init = (j - 1) / self.degree # normalized time
# Expression for the state derivative at the collocation point
xp_j = 0
for r in range(self.degree + 1):
xp_j += C[r, j] * x[r]
# Append collocation equations
f_j = self.fun(x[j], self.get_u(u, t_norm_init), params)[:,
self.idx]
x_irk_points_eq.append(h * f_j - xp_j)
# Concatenate constraints
x_irk_points = vertcat(*x_irk_points)
x_irk_points_eq = vertcat(*x_irk_points_eq)
# Root-finding function, implicitly defines x_irk_points as a function of x0 and p
vfcn = Function("vfcn", [x_irk_points, x0, u, params],
[x_irk_points_eq]).expand()
# Create a implicit function instance to solve the system of equations
ifcn = rootfinder("ifcn", "newton", vfcn)
x_irk_points = ifcn(self.CX(), x0, u, params)
x = [
x0 if r == 0 else x_irk_points[(r - 1) * nx:r * nx]
for r in range(self.degree + 1)
]
# Get an expression for the state at the end of the finite element
xf = self.CX.zeros(nx, self.degree + 1) # 0 #
for r in range(self.degree + 1):
xf[:, r] = xf[:, r - 1] + D[r] * x[r]
return xf[:, -1], horzcat(x0, xf[:, -1])
def _set_penalty_function(self, all_pn: Union[PenaltyNodeList, list,
tuple], fcn: Union[MX, SX]):
"""
Finalize the preparation of the penalty (setting function and weighted_function)
Parameters
----------
all_pn: PenaltyNodeList
The nodes
fcn: Union[MX, SX]
The value of the penalty function
"""
# Sanity checks
if self.transition and self.explicit_derivative:
raise ValueError(
"transition and explicit_derivative cannot be true simultaneously"
)
if self.transition and self.derivative:
raise ValueError(
"transition and derivative cannot be true simultaneously")
if self.derivative and self.explicit_derivative:
raise ValueError(
"derivative and explicit_derivative cannot be true simultaneously"
)
if self.multinode_constraint or self.transition:
ocp = all_pn[0].ocp
nlp = all_pn[0].nlp
nlp_post = all_pn[1].nlp
name = self.name.replace("->", "_").replace(" ",
"_").replace(",", "_")
states_pre = nlp.states.cx_end
states_post = nlp_post.states.cx
controls_pre = nlp.controls.cx_end
controls_post = nlp_post.controls.cx
state_cx = vertcat(states_pre, states_post)
control_cx = vertcat(controls_pre, controls_post)
else:
ocp = all_pn.ocp
nlp = all_pn.nlp
name = self.name
if self.integrate:
state_cx = horzcat(*([all_pn.nlp.states.cx] +
all_pn.nlp.states.cx_intermediates_list))
control_cx = all_pn.nlp.controls.cx
else:
state_cx = all_pn.nlp.states.cx
control_cx = all_pn.nlp.controls.cx
if self.explicit_derivative:
if self.derivative:
raise RuntimeError(
"derivative and explicit_derivative cannot be simultaneously true"
)
state_cx = horzcat(state_cx, all_pn.nlp.states.cx_end)
control_cx = horzcat(control_cx, all_pn.nlp.controls.cx_end)
param_cx = nlp.cx(nlp.parameters.cx)
# Do not use nlp.add_casadi_func because all functions must be registered
self.function = biorbd.to_casadi_func(name,
fcn[self.rows, self.cols],
state_cx,
control_cx,
param_cx,
expand=self.expand)
self.function_non_threaded = self.function
if self.derivative:
state_cx = horzcat(all_pn.nlp.states.cx_end, all_pn.nlp.states.cx)
control_cx = horzcat(all_pn.nlp.controls.cx_end,
all_pn.nlp.controls.cx)
self.function = biorbd.to_casadi_func(
f"{name}",
self.function(all_pn.nlp.states.cx_end,
all_pn.nlp.controls.cx_end, param_cx) -
self.function(all_pn.nlp.states.cx, all_pn.nlp.controls.cx,
param_cx),
state_cx,
control_cx,
param_cx,
)
modified_fcn = self.function(state_cx, control_cx, param_cx)
dt_cx = nlp.cx.sym("dt", 1, 1)
weight_cx = nlp.cx.sym("weight", 1, 1)
target_cx = nlp.cx.sym("target", modified_fcn.shape)
modified_fcn = modified_fcn - target_cx
if self.weight:
modified_fcn = modified_fcn**2 if self.quadratic else modified_fcn
modified_fcn = weight_cx * modified_fcn * dt_cx
else:
modified_fcn = modified_fcn * dt_cx
# Do not use nlp.add_casadi_func because all of them must be registered
self.weighted_function = Function(
name,
[state_cx, control_cx, param_cx, weight_cx, target_cx, dt_cx],
[modified_fcn])
self.weighted_function_non_threaded = self.weighted_function
if ocp.n_threads > 1 and self.multi_thread and len(self.node_idx) > 1:
self.function = self.function.map(len(self.node_idx), "thread",
ocp.n_threads)
self.weighted_function = self.weighted_function.map(
len(self.node_idx), "thread", ocp.n_threads)
else:
self.multi_thread = False # Override the multi_threading, since only one node is optimized
if self.expand:
self.function = self.function.expand()
self.weighted_function = self.weighted_function.expand()
def __set_costs(self, ocp):
if ocp.nb_phases != 1:
raise NotImplementedError("ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
raise RuntimeError("LINEAR_LS is not interfaced yet.")
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J])
else:
self.y_ref.append([np.zeros((J_tp["val"].numel(), 1)) for J_tp in J])
elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())
)
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]))
if J[0]["target"] is not None:
self.y_ref_end.append(
[J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"]
)
else:
self.y_ref_end.append([0] * (J[0]["val"].numel()))
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.J, for now.
if self.params:
for j, J in enumerate(ocp.J):
mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e, np.diag(([J[0]["objective"].weight] * J[0]["val"].numel()))
)
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]))
if J[0]["target"] is not None:
self.y_ref_end.append(
[J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"]
)
else:
self.y_ref_end.append([0] * (J[0]["val"].numel()))
# Set costs
self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel() else SX(1, 1)
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel() else SX(1, 1)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0]
# Set weight
self.acados_ocp.cost.W = np.zeros((1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros((1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros((self.acados_ocp.cost.W.shape[0],))
self.acados_ocp.cost.yref_e = np.zeros((self.acados_ocp.cost.W_e.shape[0],))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError("External is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
def add_or_replace_to_penalty_pool(self, ocp, _):
"""
Doing some configuration on the parameter and add it to the list of parameter_to_optimize
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
_: Any
The place holder for what is supposed to be nlp
"""
old_parameter_cx = ocp.v.parameters_in_list.cx
ocp.v.add_parameter(self)
# Express the previously defined parameters with the new param set
state_cx = ocp.cx()
controls_cx = ocp.cx()
parameter_cx = ocp.v.parameters_in_list.cx
for p in ocp.v.parameters_in_list:
for p_list in p.penalty_list[0]:
if p_list.weighted_function is None:
continue
dt_cx = ocp.cx.sym("dt", 1, 1)
weight_cx = ocp.cx.sym("weight", 1, 1)
target_cx = ocp.cx.sym("target",
p_list.weighted_function.numel_out(), 1)
p_list.function = Function(
p_list.function.name(),
[state_cx, controls_cx, parameter_cx],
[p_list.function(state_cx, controls_cx, old_parameter_cx)],
)
p_list.weighted_function = Function(
p_list.function.name(),
[
state_cx, controls_cx, parameter_cx, weight_cx,
target_cx, dt_cx
],
[
p_list.weighted_function(state_cx, controls_cx,
old_parameter_cx, weight_cx,
target_cx, dt_cx)
],
)
if self.penalty_list:
if ocp.phase_transitions:
raise NotImplementedError(
"Updating parameters while having phase_transition is not supported yet"
)
if isinstance(self.penalty_list, Objective):
penalty_list_tp = ObjectiveList()
penalty_list_tp.add(self.penalty_list)
self.penalty_list = penalty_list_tp
elif not isinstance(self.penalty_list, ObjectiveList):
raise RuntimeError(
"penalty_list should be built from an Objective or ObjectiveList"
)
if len(self.penalty_list) > 1 or len(self.penalty_list[0]) > 1:
raise NotImplementedError(
"Parameters with more that one penalty is not implemented yet"
)
for penalty in self.penalty_list[0]:
# Sanity check
if not isinstance(penalty.type, ObjectiveFcn.Parameter):
raise RuntimeError(
"Parameters should be declared custom_type=ObjectiveFcn.Parameters"
)
if penalty.node != Node.DEFAULT:
raise RuntimeError(
"Parameters are timeless optimization, node=Node.DEFAULT should be declared"
)
func = penalty.custom_function
all_pn = PenaltyNodeList(ocp, None, [], [], [], [])
val = func(ocp, self.cx * self.scaling, **penalty.params)
self.set_penalty(ocp, penalty, val, target_ns=1)
penalty.clear_penalty(ocp, None)
penalty._add_penalty_to_pool(all_pn)
def __set_costs(self, ocp):
# set weight for states and controls (default: 1.00)
# Q = 1.00 * np.eye(self.acados_ocp.dims.nx)
# R = 1.00 * np.eye(self.acados_ocp.dims.nu)
# self.acados_ocp.cost.W = linalg.block_diag(Q, R)
# self.acados_ocp.cost.W_e = Q
self.y_ref = []
self.y_ref_end = []
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
# set Lagrange terms
self.acados_ocp.cost.Vx = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nx))
self.acados_ocp.cost.Vx[: self.acados_ocp.dims.nx, :] = np.eye(self.acados_ocp.dims.nx)
Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu))
Vu[self.acados_ocp.dims.nx :, :] = np.eye(self.acados_ocp.dims.nu)
self.acados_ocp.cost.Vu = Vu
# set Mayer term
self.acados_ocp.cost.Vx_e = np.zeros((self.acados_ocp.dims.nx, self.acados_ocp.dims.nx))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
if ocp.nb_phases != 1:
# TODO: Please confirm this
raise NotImplementedError("ACADOS with more than one phase is not implemented yet")
for i in range(ocp.nb_phases):
# TODO: I think ocp.J is missing here (the parameters would be stored there)
for j, J in enumerate(ocp.nlp[i]["J"]):
if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J])
else:
raise RuntimeError("Should we put y_ref = zeros?")
elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction:
raise RuntimeError("TODO: I may have broken this (is this the right J?)")
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J[0]["val"]])
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0]))
if J[0]["target"] is not None:
self.y_ref_end.append([J[0]["target"]])
else:
raise RuntimeError("TODO: Should we put y_ref_end = zeros?")
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
if self.lagrange_costs.numel():
self.acados_ocp.model.cost_y_expr = self.lagrange_costs
else:
self.acados_ocp.model.cost_y_expr = SX(1, 1)
if self.mayer_costs.numel():
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs
else:
self.acados_ocp.model.cost_y_expr_e = SX(1, 1)
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0]
self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny, 1),))
self.acados_ocp.cost.yref_e = np.zeros((max(self.acados_ocp.dims.ny_e, 1),))
# TODO changed hard coded values below (you can use J["weight"])
Q_ocp = np.zeros((15, 15))
np.fill_diagonal(Q_ocp, 1000)
R_ocp = np.zeros((4, 4))
np.fill_diagonal(R_ocp, 1000)
self.acados_ocp.cost.W = linalg.block_diag(Q_ocp, R_ocp)
self.acados_ocp.cost.W_e = np.zeros((1, 1))
# TODO: Is the following useful?
# if len(self.y_ref):
# self.y_ref = np.vstack(self.y_ref)
# else:
# self.y_ref = [np.zeros((1, 1))] * self.ocp
# if len(self.y_ref_end):
# self.y_ref_end = np.vstack(self.y_ref_end)
# else:
# self.y_ref_end = np.zeros((1, 1))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
for i in range(ocp.nb_phases):
for j in range(len(ocp.nlp[i]["J"])):
J = ocp.nlp[i]["J"][j][0]
raise RuntimeError("TODO: The target is not right currently")
if J["type"] == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J["val"][0] - J["target"][0])
elif J["type"] == ObjectiveFunction.MayerFunction:
raise RuntimeError("TODO: I may have broken this (is this the right J?)")
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J["val"]])
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0]))
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
self.acados_ocp.model.cost_expr_ext_cost = sum1(self.lagrange_costs)
self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs)
else:
raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
[biorbd_model.muscleNames()[i].to_string() for i in range(18)],
rotation=45)
plt.bar(x2, height, width, color='blue')
plt.xlabel('muscles')
plt.ylabel('weight')
plt.title('weight on force iso max')
plt.legend(["Acados", "Ipopt"], loc=2)
plt.show()
# ---- Tau --- #
symbolic_states = MX.sym("x", biorbd_model.nbQ() * 2, 1)
symbolic_controls = MX.sym("a", biorbd_model.nbMuscles(), 1)
torque_func = Function(
"MuscleTau",
[symbolic_states, symbolic_controls],
[muscles_tau(symbolic_states, symbolic_controls, nlp_ac)],
["x", "a"],
["torque"],
).expand()
torques_ac = np.ndarray((biorbd_model.nbQ(), n_shooting_points))
torques_ac[:, :] = torque_func(x_ac[:, :], u_ac[:, :])
symbolic_states = MX.sym("x", biorbd_model.nbQ() * 2, 1)
symbolic_controls = MX.sym("a", biorbd_model.nbMuscles(), 1)
torque_func = Function(
"MuscleTau",
[symbolic_states, symbolic_controls],
[muscles_tau(symbolic_states, symbolic_controls, nlp_ip)],
["x", "a"],
["torque"],
).expand()
q, qdot, tau, mus = ProblemType.get_data_from_V(ocp, sol["x"])
n_q = ocp.nlp[0]["model"].nbQ()
n_mark = ocp.nlp[0]["model"].nbMarkers()
n_frames = q.shape[1]
mus_act = mus[0]
mus_exci = np.array(mus[1])
markers = np.ndarray((3, n_mark, q.shape[1]))
markers_func = []
for i in range(n_mark):
markers_func.append(
Function(
"ForwardKin",
[ocp.symbolic_states],
[biorbd_model.marker(ocp.symbolic_states[:n_q], i).to_mx()],
["q"],
["marker_" + str(i)],
).expand())
for i in range(n_frames):
for j, mark_func in enumerate(markers_func):
markers[:, j, i] = np.array(
mark_func(
np.append(np.append(q[:, i], qdot[:, i]),
mus_act[0][:, i]))).squeeze()
plt.figure("Markers")
for i in range(markers.shape[1]):
plt.plot(np.linspace(0, 2, n_shooting_points + 1),
markers_ref[:, i, :].T, "k")
plt.plot(np.linspace(0, 2, n_shooting_points + 1), markers[:, i, :].T,
def RK4(ode, ode_opt):
"""
Numerical integration using fourth order Runge-Kutta method.
:param ode: ode["x"] -> States. ode["p"] -> Controls. ode["ode"] -> Ordinary differential equation function
(dynamics of the system).
:param ode_opt: ode_opt["t0"] -> Initial time of the integration. ode_opt["tf"] -> Final time of the integration.
ode_opt["number_of_finite_elements"] -> Number of steps between nodes. ode_opt["idx"] -> Index of ??. (integer)
:return: Integration function. (CasADi function)
"""
t_span = ode_opt["t0"], ode_opt["tf"]
n_step = ode_opt["number_of_finite_elements"]
idx = ode_opt["idx"]
CX = ode_opt["CX"]
x_sym = ode["x"]
u_sym = ode["p"]
param_sym = ode_opt["param"]
fun = ode["ode"]
model = ode_opt["model"]
step_time = t_span[1] - t_span[0]
h_norm = 1 / n_step
h = step_time * h_norm # Length of steps
control_type = ode_opt["control_type"]
def get_u(u, dt_norm):
if control_type == ControlType.CONSTANT:
return u
elif control_type == ControlType.LINEAR_CONTINUOUS:
return u[:, 0] + (u[:, 1] - u[:, 0]) * dt_norm
else:
raise RuntimeError(f"{control_type} ControlType not implemented yet")
def dxdt(h, states, controls, params):
u = controls
x = CX(states.shape[0], n_step + 1)
p = params
x[:, 0] = states
nb_dof = 0
quat_idx = []
quat_number = 0
for j in range(model.nbSegment()):
if model.segment(j).isRotationAQuaternion():
quat_idx.append([nb_dof, nb_dof + 1, nb_dof + 2, model.nbDof() + quat_number])
quat_number += 1
nb_dof += model.segment(j).nbDof()
for i in range(1, n_step + 1):
t_norm_init = (i - 1) / n_step # normalized time
k1 = fun(x[:, i - 1], get_u(u, t_norm_init), p)[:, idx]
k2 = fun(x[:, i - 1] + h / 2 * k1, get_u(u, t_norm_init + h_norm / 2), p)[:, idx]
k3 = fun(x[:, i - 1] + h / 2 * k2, get_u(u, t_norm_init + h_norm / 2), p)[:, idx]
k4 = fun(x[:, i - 1] + h * k3, get_u(u, t_norm_init + h_norm), p)[:, idx]
x[:, i] = x[:, i - 1] + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
for j in range(model.nbQuat()):
quaternion = vertcat(
x[quat_idx[j][3], i], x[quat_idx[j][0], i], x[quat_idx[j][1], i], x[quat_idx[j][2], i]
)
quaternion /= norm_fro(quaternion)
x[quat_idx[j][0] : quat_idx[j][2] + 1, i] = quaternion[1:4]
x[quat_idx[j][3], i] = quaternion[0]
return x[:, -1], x
return Function(
"integrator", [x_sym, u_sym, param_sym], dxdt(h, x_sym, u_sym, param_sym), ["x0", "p", "params"], ["xf", "xall"]
)
# Eingang symbolisch definieren
u_1 = SX.sym('u_1')
u_2 = SX.sym("u_2")
# Eingangsverktor
controls = vertcat(u_1, u_2)
# kriegen die Anzahl der Zeiler("shape" ist ein Tuple)
n_states = states.shape[0] # p 22.
# Eingangsanzahl
n_controls = controls.shape[0]
rhs = vertcat(sin(theta) * u_1, cos(theta) * u_1, u_2)
f = Function('f', [states, controls], [rhs * T + states])
K = SX([[0.99581, 0.086879, 0.1499]])
# K = SX([[31.2043, 2.7128, 0.4824]])
phi_c = A_d - B_d @ K
P_f = SX([[5959.1, 517.92, 65.058], [517.92, 51.198, 5.6392],
[65.058, 5.6392, 641.7]])
# P_f=SX([[68639,5891.4,616.42],[5891.4,519.84,53.599],[616.42,53.599,641.7]])
state_r = SX([[0.8], [0], [48.760258862]])
u_r = [157.291157619]
Q = SX.zeros(3, 3)
# Q[0, 0] = 1e3
Q[0, 0] = 1
Q[1, 1] = 1
Q[2, 2] = 1
# muscles_tau = model.muscularJointTorque(muscles_states, True, q, qdot).to_mx()
muscles_force = model.muscleForces(muscles_states, q, qdot).to_mx()
return muscles_force
seaborn.set_style("whitegrid")
seaborn.color_palette()
biorbd_model = biorbd.Model("arm_wt_rot_scap.bioMod")
qMX = MX.sym("qMX", biorbd_model.nbQ(), 1)
dqMX = MX.sym("dqMX", biorbd_model.nbQ(), 1)
aMX = MX.sym("aMX", biorbd_model.nbMuscles(), 1)
uMX = MX.sym("uMX", biorbd_model.nbMuscles(), 1)
force_func = Function(
"MuscleForce",
[qMX, dqMX, aMX, uMX],
[muscles_forces(qMX, dqMX, aMX, uMX, biorbd_model)],
["qMX", "dqMX", "aMX", "uMX"],
["Force"],
).expand()
T = 8
Ns = 800
motion = "REACH2"
nb_try = 30
marker_noise_lvl = [0, 0.002, 0.005, 0.01]
EMG_noise_lvl = [0, 1.5, 2, 2.5]
co_lvl = 4
TRACK_EMG = True
if TRACK_EMG:
with_emg = "w"
else:
def optProblem(x, u, x0_measure, N, params):
NT = params['dist']['NT']
Uf = params['dist']['F_0']
#Modeling the system
_, state, xdot, inputs = ColCSTR_model(Uf, params)
f = Function('f', [state, inputs], [xdot])
#Unpacking parameters
x_min = params['bounds']['x_min']
x_max = params['bounds']['x_max']
#Loading steady state data
data = spio.loadmat('CstrDistXinit.mat', squeeze_me=True)
Xinit = data['Xinit']
xf = Xinit[0:84]
u_opt = Xinit[84:89]
#Problem dimensions
nx = params['prob']['nx'] #Number of states
nu = params['prob']['nu'] #Number of inputs
nk = params['prob']['nk']
tf = params['prob']['tf']
h = params['prob']['h']
ns = params['prob']['ns']
#Collecting model variables
u = tile(u, nk)
model = {
'NT': NT,
'f': f,
'xdot_val_rf_ss': xf,
'x': x,
'u_opt': u_opt,
'u': u
}
params['model'] = model
#Preparing collocation matrices
_, C, D, d = collocationSetup()
params['prob']['d'] = d
#Collecting collocation variables
colloc = {'C': C, 'D': D, 'h': h}
params['colloc'] = colloc
#Empty NLP
w = MX() #Decision variables (control + state)
w0 = [] #Initial guess
lbw = [] #Lower bound for decision variable
ubw = [] #Upper bound for decision variable
g = MX() #Nonlinear constraint
lbg = [] #Lower bound for nonlinear constraint
ubg = [] #Upper bound for nonlinear constraint
J = 0 #Initialize objective function
#Weight variables
delta_t = 1
alpha = 1
beta = 1
gamma = 1
weight = {'delta_t': delta_t, 'alpha': alpha, 'beta': beta, 'gamma': gamma}
params['weight'] = weight
#Initial conditions
X0 = MX.sym('X0', nx)
w = vertcat(w, X0)
w0 = [i for i in x[0, 0:nx]]
lbw = [i for i in x_min]
ubw = [i for i in x_max]
g = vertcat(g, X0 - x0_measure)
lbg = params['bounds']['lbg']
ubg = params['bounds']['ubg']
Xk = X0
data = spio.loadmat('Qmax.mat', squeeze_me=True)
Qmax = data['Qmax']
params['Qmax'] = Qmax
count = 2 #Counter for state variable
ssoftc = 0
for iter in range(0, N):
J, g, w0, w, lbg, ubg, lbw, ubw, Xk, params, count, ssoftc = itPredHorizon(
Xk, w, w0, lbw, ubw, lbg, ubg, g, J, params, iter, count, ssoftc,
d)
return J, g, w0, w, lbg, ubg, lbw, ubw, params
def __set_constraints(self, ocp):
"""
Set the constraints from the ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
# constraints handling in self.acados_ocp
u_min = np.array(ocp.nlp[0].u_bounds.min)
u_max = np.array(ocp.nlp[0].u_bounds.max)
x_min = np.array(ocp.nlp[0].x_bounds.min)
x_max = np.array(ocp.nlp[0].x_bounds.max)
self.all_constr = SX()
self.end_constr = SX()
# TODO:change for more node flexibility on bounds
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
for i in range(ocp.n_phases):
for g, G in enumerate(ocp.nlp[i].g):
if not G:
continue
if G[0]["constraint"].node[0] is Node.ALL:
self.all_constr = vertcat(self.all_constr,
G[0]["val"].reshape((-1, 1)))
self.all_g_bounds.concatenate(G[0]["bounds"])
if len(G) > ocp.nlp[0].ns:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
self.end_g_bounds.concatenate(G[0]["bounds"])
elif G[0]["constraint"].node[0] is Node.END:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
self.end_g_bounds.concatenate(G[0]["bounds"])
else:
raise RuntimeError(
"Except for states and controls, Acados solver only handles constraints on last or all nodes."
)
self.acados_model.con_h_expr = self.all_constr
self.acados_model.con_h_expr_e = self.end_constr
if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds min must be the same at each shooting point with ACADOS"
)
if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds max must be the same at each shooting point with ACADOS"
)
if (not np.isfinite(u_min).all() or not np.isfinite(x_min).all()
or not np.isfinite(u_max).all()
or not np.isfinite(x_max).all()):
raise NotImplementedError(
"u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it "
"to a big value instead.")
# setup state constraints
# TODO replace all these np.concatenate by proper bound and initial_guess classes
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
param_bounds_max = []
param_bounds_min = []
if self.params.size:
param_bounds_max = self.params.bounds.max[:, 0]
param_bounds_min = self.params.bounds.min[:, 0]
for i in range(3):
self.x_bound_max[:, i] = np.concatenate(
(param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i])))
self.x_bound_min[:, i] = np.concatenate(
(param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i])))
# setup control constraints
self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:,
0])
self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:,
0])
self.acados_ocp.constraints.idxbu = np.array(
range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# initial state constraints
self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0]
self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0]
self.acados_ocp.constraints.idxbx_0 = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# setup path state constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1]
self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1]
self.acados_ocp.constraints.idxbx = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# setup terminal state constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1]
self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1]
self.acados_ocp.constraints.idxbx_e = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
# setup algebraic constraint
self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0])
self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0])
# setup terminal algebraic constraint
self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:,
0])
self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:,
0])
def generate_c_code_external_cost(model, is_terminal):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double")
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
x = model.x
p = model.p
if isinstance(x, MX):
symbol = MX.sym
else:
symbol = SX.sym
if is_terminal:
suffix_name = "_cost_ext_cost_e_fun"
suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_e_fun_jac"
u = symbol("u", 0, 0)
ext_cost = model.cost_expr_ext_cost_e
else:
suffix_name = "_cost_ext_cost_fun"
suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost
# set up functions to be exported
fun_name = model.name + suffix_name
fun_name_hess = model.name + suffix_name_hess
fun_name_jac = model.name + suffix_name_jac
# generate jacobians
jac_x = jacobian(ext_cost, x)
jac_u = jacobian(ext_cost, u)
# generate hessians
hess_uu = jacobian(jac_u.T, u)
hess_xu = jacobian(jac_u.T, x)
hess_ux = jacobian(jac_x.T, u)
hess_xx = jacobian(jac_x.T, x)
full_hess = vertcat(horzcat(hess_uu, hess_xu), horzcat(hess_ux, hess_xx))
ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost])
ext_cost_fun_jac_hess = Function(
fun_name_hess, [x, u, p],
[ext_cost, vertcat(jac_u.T, jac_x.T), full_hess])
ext_cost_fun_jac = Function(fun_name_jac, [x, u, p],
[ext_cost, vertcat(jac_u.T, jac_x.T)])
# generate C code
if not os.path.exists("c_generated_code"):
os.mkdir("c_generated_code")
os.chdir("c_generated_code")
gen_dir = model.name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = "./" + gen_dir
os.chdir(gen_dir_location)
ext_cost_fun.generate(fun_name, casadi_opts)
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts)
ext_cost_fun_jac.generate(fun_name_jac, casadi_opts)
os.chdir("../..")
return
def __set_costs(self, ocp):
"""
Set the cost functions from ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
if ocp.n_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
ctrl_objs = [
PenaltyType.MINIMIZE_TORQUE,
PenaltyType.MINIMIZE_MUSCLES_CONTROL,
PenaltyType.MINIMIZE_ALL_CONTROLS,
]
state_objs = [
PenaltyType.MINIMIZE_STATE,
]
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
self.Vu = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nu)
self.Vx = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx)
self.Vxe = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx)
for i in range(ocp.n_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
if J[0]["objective"].type.value[0] in ctrl_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nu)))
vu = np.zeros(ocp.nlp[0].nu)
vu[index] = 1.0
self.Vu = np.vstack((self.Vu, np.diag(vu)))
self.Vx = np.vstack(
(self.Vx,
np.zeros((ocp.nlp[0].nu, ocp.nlp[0].nx))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nu))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nu, 1))
y_ref_tp = []
for J_tp in J:
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append(
[np.zeros((ocp.nlp[0].nu, 1)) for _ in J])
elif J[0]["objective"].type.value[0] in state_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vx = np.zeros(ocp.nlp[0].nx)
vx[index] = 1.0
self.Vx = np.vstack((self.Vx, np.diag(vx)))
self.Vu = np.vstack(
(self.Vu,
np.zeros((ocp.nlp[0].nx, ocp.nlp[0].nu))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_ref_tp = []
for J_tp in J:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append(
[np.zeros((ocp.nlp[0].nx, 1)) for _ in J])
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term with "
f"LINEAR_LS cost type")
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vxe = np.zeros(ocp.nlp[0].nx)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(
np.zeros((ocp.nlp[0].nx, 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
if J[0]["objective"].type.value[0] in state_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vxe = np.zeros(ocp.nlp[0].nx)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(
np.zeros((ocp.nlp[0].nx, 1)))
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term "
f"with LINEAR_LS cost type")
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.params.size:
raise RuntimeError(
"Params not yet handled with LINEAR_LS cost type")
# Set costs
self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[
0] else np.zeros((0, 0))
# Set dimensions
self.acados_ocp.dims.ny = sum(
[len(data[0]) for data in self.y_ref])
self.acados_ocp.dims.ny_e = sum(
[len(data) for data in self.y_ref_end])
# Set weight
self.acados_ocp.cost.W = self.W
self.acados_ocp.cost.W_e = self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i in range(ocp.n_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(
J[-1]["target"].T.reshape((-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[-1]["val"].numel(), 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.J, for now.
if self.params.size:
for j, J in enumerate(ocp.J):
mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag(([J[0]["objective"].weight] *
J[0]["val"].numel())))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
# Set costs
self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel(
) else SX(1, 1)
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel(
) else SX(1, 1)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
# Set weight
self.acados_ocp.cost.W = np.zeros(
(1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros(
(1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError(
"EXTERNAL is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def test_multistep_trajectory(before_test_onestep_reachability):
""" Compare multi-steps 'by hand' with the function """
mu_0, _, gp, k_fb, k_ff, L_mu, L_sigm, c_safety, a, b = before_test_onestep_reachability
T = 3
n_u, n_s = np.shape(k_fb)
k_fb_cas_single = SX.sym("k_fb_single", (n_u, n_s))
k_ff_cas_single = SX.sym("k_ff_single", (n_u, 1))
k_ff_cas_all = SX.sym("k_ff_single", (T, n_u))
k_fb_cas_all = SX.sym("k_fb_all", (T - 1, n_s * n_u))
k_fb_cas_all_inp = [
k_fb_cas_all[i, :].reshape((n_u, n_s)) for i in range(T - 1)
]
mu_0_cas = SX.sym("mu_0", (n_s, 1))
sigma_0_cas = SX.sym("sigma_0", (n_s, n_s))
mu_onestep_no_var_in, sigm_onestep_no_var_in, _ = prop_casadi.one_step_taylor(
mu_0_cas, gp, k_ff_cas_single, a=a, b=b)
mu_one_step, sigm_onestep, _ = prop_casadi.one_step_taylor(
mu_0_cas,
gp,
k_ff_cas_single,
k_fb=k_fb_cas_single,
sigma_x=sigma_0_cas,
a=a,
b=b)
mu_multistep, sigma_multistep, _ = prop_casadi.multi_step_taylor_symbolic(
mu_0_cas, gp, k_ff_cas_all, k_fb_cas_all_inp, a=a, b=b)
on_step_no_var_in = Function(
"on_step_no_var_in", [mu_0_cas, k_ff_cas_single],
[mu_onestep_no_var_in, sigm_onestep_no_var_in])
one_step = Function(
"one_step", [mu_0_cas, sigma_0_cas, k_ff_cas_single, k_fb_cas_single],
[mu_one_step, sigm_onestep])
multi_step = Function("multi_step", [mu_0_cas, k_ff_cas_all, k_fb_cas_all],
[mu_multistep, sigma_multistep])
# TODO: Need mu, sigma as input aswell
mu_1, sigma_1 = on_step_no_var_in(mu_0, k_ff)
mu_2, sigma_2 = one_step(mu_1, sigma_1, k_ff, k_fb)
mu_3, sigma_3 = one_step(mu_2, sigma_2, k_ff, k_fb)
# TODO: stack k_ff and k_fb
k_fb_mult = np.array(cas_reshape(k_fb, (1, n_u * n_s)))
k_fb_mult = np.array(vertcat(*[k_fb_mult] * (T - 1)))
k_ff_mult = vertcat(*[k_ff] * T)
mu_all, sigma_all = multi_step(mu_0, k_ff_mult, k_fb_mult)
assert np.allclose(
np.array(mu_all[0, :]),
np.array(mu_1).T), "Are the centers of the first prediction the same?"
assert np.allclose(
np.array(mu_all[-1, :]),
np.array(mu_3).T), "Are the centers of the final prediction the same?"
assert np.allclose(cas_reshape(sigma_all[-1, :], (n_s, n_s)),
sigma_3), "Are the last covariance matrices the same?"
class PenaltyOption(OptionGeneric):
"""
A placeholder for a penalty
Attributes
----------
node: Node
The node within a phase on which the penalty is acting on
quadratic: bool
If the penalty is quadratic
rows: Union[list, tuple, range, np.ndarray]
The index of the rows in the penalty to keep
cols: Union[list, tuple, range, np.ndarray]
The index of the columns in the penalty to keep
expand: bool
If the penalty should be expanded or not
target: np.array(target)
A target to track for the penalty
target_plot_name: str
The plot name of the target
target_to_plot: np.ndarray
The subset of the target to plot
plot_target: bool
If the target should be plotted
custom_function: Callable
A user defined function to call to get the penalty
node_idx: Union[list, tuple, Node]
The index in nlp to apply the penalty to
dt: float
The delta time
function: Function
The casadi function of the penalty
weighted_function: Function
The casadi function of the penalty weighted
derivative: bool
If the minimization is applied on the numerical derivative of the state [f(t+1) - f(t)]
explicit_derivative: bool
If the minimization is applied to derivative of the penalty [f(t, t+1)]
integration_rule: IntegralApproximation
The integration rule to use for the penalty
transition: bool
If the penalty is a transition
phase_pre_idx: int
The index of the nlp of pre when penalty is transition
phase_post_idx: int
The index of the nlp of post when penalty is transition
constraint_type: ConstraintType
If the penalty is from the user or from bioptim (implicit or internal)
multi_thread: bool
If the penalty is multithreaded
Methods
-------
set_penalty(self, penalty: Union[MX, SX], all_pn: PenaltyNodeList)
Prepare the dimension and index of the penalty (including the target)
_set_dim_idx(self, dim: Union[list, tuple, range, np.ndarray], n_rows: int)
Checks if the variable index is consistent with the requested variable.
_check_target_dimensions(self, all_pn: PenaltyNodeList, n_time_expected: int)
Checks if the variable index is consistent with the requested variable.
If the function returns, all is okay
_set_penalty_function(self, all_pn: Union[PenaltyNodeList, list, tuple], fcn: Union[MX, SX])
Finalize the preparation of the penalty (setting function and weighted_function)
add_target_to_plot(self, all_pn: PenaltyNodeList, combine_to: str)
Interface to the plot so it can be properly added to the proper plot
_finish_add_target_to_plot(self, all_pn: PenaltyNodeList)
Internal interface to add (after having check the target dimensions) the target to the plot if needed
add_or_replace_to_penalty_pool(self, ocp, nlp)
Doing some configuration on the penalty and add it to the list of penalty
_add_penalty_to_pool(self, all_pn: PenaltyNodeList)
Return the penalty pool for the specified penalty (abstract)
clear_penalty(self, ocp, nlp)
Resets a penalty. A negative penalty index creates a new empty penalty (abstract)
_get_penalty_node_list(self, ocp, nlp) -> PenaltyNodeList
Get the actual node (time, X and U) specified in the penalty
"""
def __init__(
self,
penalty: Any,
phase: int = 0,
node: Union[Node, list, tuple] = Node.DEFAULT,
target: Union[int, float, np.array, list[int], list[float], list[np.array]] = None,
quadratic: bool = None,
weight: float = 1,
derivative: bool = False,
explicit_derivative: bool = False,
integrate: bool = False,
integration_rule: IntegralApproximation = IntegralApproximation.DEFAULT,
index: list = None,
rows: Union[list, tuple, range, np.ndarray] = None,
cols: Union[list, tuple, range, np.ndarray] = None,
states_mapping: BiMapping = None,
custom_function: Callable = None,
constraint_type: ConstraintType = ConstraintType.USER,
multi_thread: bool = None,
expand: bool = False,
**params: Any,
):
"""
Parameters
----------
penalty: PenaltyType
The actual penalty
phase: int
The phase the penalty is acting on
node: Union[Node, list, tuple]
The node within a phase on which the penalty is acting on
target: Union[int, float, np.array, list[int], list[float], list[np.array]]
A target to track for the penalty
quadratic: bool
If the penalty is quadratic
weight: float
The weighting applied to this specific penalty
derivative: bool
If the function should be evaluated at X and X+1
explicit_derivative: bool
If the function should be evaluated at [X, X+1]
integrate: bool
If the function should be integrated
integration_rule: IntegralApproximation
The rule to use for the integration
index: int
The component index the penalty is acting on
custom_function: Callable
A user defined function to call to get the penalty
constraint_type: ConstraintType
If the penalty is from the user or from bioptim (implicit or internal)
**params: dict
Generic parameters for the penalty
"""
super(PenaltyOption, self).__init__(phase=phase, type=penalty, **params)
self.node: Union[Node, list, tuple] = node
self.quadratic = quadratic
self.integration_rule = integration_rule
if index is not None and rows is not None:
raise ValueError("rows and index cannot be defined simultaneously since they are the same variable")
self.rows = rows if rows is not None else index
self.cols = cols
self.expand = expand
self.target = None
if target is not None:
target = np.array(target)
if isinstance(target, int) or isinstance(target, float) or isinstance(target, np.ndarray):
target = [target]
self.target = []
for t in target:
self.target.append(np.array(t))
if len(self.target[-1].shape) == 0:
self.target[-1] = self.target[-1][np.newaxis]
if len(self.target[-1].shape) == 1:
self.target[-1] = self.target[-1][:, np.newaxis]
if len(self.target) == 1 and (
self.integration_rule == IntegralApproximation.TRAPEZOIDAL
or self.integration_rule == IntegralApproximation.TRUE_TRAPEZOIDAL
):
if self.node == Node.ALL or self.node == Node.DEFAULT:
self.target = [self.target[0][:, :-1], self.target[0][:, 1:]]
else:
raise NotImplementedError(
f"A list of 2 elements is required with {self.node} and TRAPEZOIDAL Integration"
f"except for Node.NODE_ALL and Node.NODE_DEFAULT"
"which can be automatically generated"
)
self.target_plot_name = None
self.target_to_plot = None
# todo: not implemented yet for trapezoidal integration
self.plot_target = (
False
if (
self.integration_rule == IntegralApproximation.TRAPEZOIDAL
or self.integration_rule == IntegralApproximation.TRUE_TRAPEZOIDAL
)
else True
)
self.states_mapping = states_mapping
self.custom_function = custom_function
self.node_idx = []
self.dt = 0
self.weight = weight
self.function: Union[Function, None] = None
self.function_non_threaded: Union[Function, None] = None
self.weighted_function: Union[Function, None] = None
self.weighted_function_non_threaded: Union[Function, None] = None
self.derivative = derivative
self.explicit_derivative = explicit_derivative
self.integrate = integrate
self.transition = False
self.multinode_constraint = False
self.phase_pre_idx = None
self.phase_post_idx = None
if self.derivative and self.explicit_derivative:
raise ValueError("derivative and explicit_derivative cannot be both True")
self.constraint_type = constraint_type
self.multi_thread = multi_thread
def set_penalty(self, penalty: Union[MX, SX], all_pn: PenaltyNodeList):
"""
Prepare the dimension and index of the penalty (including the target)
Parameters
----------
penalty: Union[MX, SX],
The actual penalty function
all_pn: PenaltyNodeList
The penalty node elements
"""
self.rows = self._set_dim_idx(self.rows, penalty.rows())
self.cols = self._set_dim_idx(self.cols, penalty.columns())
if self.target is not None:
self._check_target_dimensions(all_pn, len(all_pn.t))
if self.plot_target:
self._finish_add_target_to_plot(all_pn)
self._set_penalty_function(all_pn, penalty)
self._add_penalty_to_pool(all_pn)
def _set_dim_idx(self, dim: Union[list, tuple, range, np.ndarray], n_rows: int):
"""
Checks if the variable index is consistent with the requested variable.
Parameters
----------
dim: Union[list, tuple, range]
The dimension to set
n_rows: int
The expected row shape
Returns
-------
The formatted indices
"""
if dim is None:
dim = range(n_rows)
else:
if isinstance(dim, int):
dim = [dim]
if max(dim) > n_rows:
raise RuntimeError(f"{self.name} index cannot be higher than nx ({n_rows})")
dim = np.array(dim)
if not np.issubdtype(dim.dtype, np.integer):
raise RuntimeError(f"{self.name} index must be a list of integer")
return dim
def _check_target_dimensions(self, all_pn: PenaltyNodeList, n_time_expected: int):
"""
Checks if the variable index is consistent with the requested variable.
If the function returns, all is okay
Parameters
----------
all_pn: PenaltyNodeList
The penalty node elements
n_time_expected: Union[list, tuple]
The expected shape (n_rows, ns) of the data to track
"""
if self.integration_rule == IntegralApproximation.RECTANGLE:
n_dim = len(self.target[0].shape)
if n_dim != 2 and n_dim != 3:
raise RuntimeError(
f"target cannot be a vector (it can be a matrix with time dimension equals to 1 though)"
)
if self.target[0].shape[-1] == 1:
self.target = np.repeat(self.target, n_time_expected, axis=-1)
shape = (
(len(self.rows), n_time_expected) if n_dim == 2 else (len(self.rows), len(self.cols), n_time_expected)
)
if self.target[0].shape != shape:
raise RuntimeError(
f"target {self.target[0].shape} does not correspond to expected size {shape} for penalty {self.name}"
)
# If the target is on controls and control is constant, there will be one value missing
if all_pn is not None:
if (
all_pn.nlp.control_type == ControlType.CONSTANT
and all_pn.nlp.ns in all_pn.t
and self.target[0].shape[-1] == all_pn.nlp.ns
):
if all_pn.t[-1] != all_pn.nlp.ns:
raise NotImplementedError("Modifying target for END not being last is not implemented yet")
self.target[0] = np.concatenate(
(self.target[0], np.nan * np.zeros((self.target[0].shape[0], 1))), axis=1
)
elif (
self.integration_rule == IntegralApproximation.TRAPEZOIDAL
or self.integration_rule == IntegralApproximation.TRAPEZOIDAL
):
target_dim = len(self.target)
if target_dim != 2:
raise RuntimeError(f"targets with trapezoidal integration rule need to get a list of two elements.")
for target in self.target:
n_dim = len(target.shape)
if n_dim != 2 and n_dim != 3:
raise RuntimeError(
f"target cannot be a vector (it can be a matrix with time dimension equals to 1 though)"
)
if target.shape[-1] == 1:
target = np.repeat(target, n_time_expected, axis=-1)
shape = (
(len(self.rows), n_time_expected - 1)
if n_dim == 2
else (len(self.rows), len(self.cols), n_time_expected - 1)
)
for target in self.target:
if target.shape != shape:
raise RuntimeError(
f"target {target.shape} does not correspond to expected size {shape} for penalty {self.name}"
)
# If the target is on controls and control is constant, there will be one value missing
if all_pn is not None:
if (
all_pn.nlp.control_type == ControlType.CONSTANT
and all_pn.nlp.ns in all_pn.t
and self.target[0].shape[-1] == all_pn.nlp.ns - 1
and self.target[1].shape[-1] == all_pn.nlp.ns - 1
):
if all_pn.t[-1] != all_pn.nlp.ns:
raise NotImplementedError("Modifying target for END not being last is not implemented yet")
self.target = np.concatenate((self.target, np.nan * np.zeros((self.target.shape[0], 1))), axis=1)
def _set_penalty_function(self, all_pn: Union[PenaltyNodeList, list, tuple], fcn: Union[MX, SX]):
"""
Finalize the preparation of the penalty (setting function and weighted_function)
Parameters
----------
all_pn: PenaltyNodeList
The nodes
fcn: Union[MX, SX]
The value of the penalty function
"""
# Sanity checks
if self.transition and self.explicit_derivative:
raise ValueError("transition and explicit_derivative cannot be true simultaneously")
if self.transition and self.derivative:
raise ValueError("transition and derivative cannot be true simultaneously")
if self.derivative and self.explicit_derivative:
raise ValueError("derivative and explicit_derivative cannot be true simultaneously")
def get_u(nlp, u: Union[MX, SX], dt: Union[MX, SX]):
"""
Get the control at a given time
Parameters
----------
nlp: NonlinearProgram
The nonlinear program
u: Union[MX, SX]
The control matrix
dt: Union[MX, SX]
The time a which control should be computed
Returns
-------
The control at a given time
"""
if nlp.control_type == ControlType.CONSTANT:
return u
elif nlp.control_type == ControlType.LINEAR_CONTINUOUS:
return u[:, 0] + (u[:, 1] - u[:, 0]) * dt
else:
raise RuntimeError(f"{nlp.control_type} ControlType not implemented yet")
return u
if self.multinode_constraint or self.transition:
ocp = all_pn[0].ocp
nlp = all_pn[0].nlp
nlp_post = all_pn[1].nlp
name = self.name.replace("->", "_").replace(" ", "_").replace(",", "_")
states_pre = nlp.states.cx_end
states_post = nlp_post.states.cx
controls_pre = nlp.controls.cx_end
controls_post = nlp_post.controls.cx
state_cx = vertcat(states_pre, states_post)
control_cx = vertcat(controls_pre, controls_post)
else:
ocp = all_pn.ocp
nlp = all_pn.nlp
name = self.name
if self.integrate:
state_cx = horzcat(*([all_pn.nlp.states.cx] + all_pn.nlp.states.cx_intermediates_list))
control_cx = all_pn.nlp.controls.cx
else:
state_cx = all_pn.nlp.states.cx
control_cx = all_pn.nlp.controls.cx
if self.explicit_derivative:
if self.derivative:
raise RuntimeError("derivative and explicit_derivative cannot be simultaneously true")
state_cx = horzcat(state_cx, all_pn.nlp.states.cx_end)
control_cx = horzcat(control_cx, all_pn.nlp.controls.cx_end)
param_cx = nlp.cx(nlp.parameters.cx)
# Do not use nlp.add_casadi_func because all functions must be registered
sub_fcn = fcn[self.rows, self.cols]
self.function = biorbd.to_casadi_func(name, sub_fcn, state_cx, control_cx, param_cx, expand=self.expand)
self.function_non_threaded = self.function
if self.derivative:
state_cx = horzcat(all_pn.nlp.states.cx_end, all_pn.nlp.states.cx)
control_cx = horzcat(all_pn.nlp.controls.cx_end, all_pn.nlp.controls.cx)
self.function = biorbd.to_casadi_func(
f"{name}",
self.function(all_pn.nlp.states.cx_end, all_pn.nlp.controls.cx_end, param_cx)
- self.function(all_pn.nlp.states.cx, all_pn.nlp.controls.cx, param_cx),
state_cx,
control_cx,
param_cx,
)
dt_cx = nlp.cx.sym("dt", 1, 1)
is_trapezoidal = (
self.integration_rule == IntegralApproximation.TRAPEZOIDAL
or self.integration_rule == IntegralApproximation.TRUE_TRAPEZOIDAL
)
target_shape = tuple(
[
len(self.rows),
len(self.cols) + 1 if is_trapezoidal else len(self.cols),
]
)
target_cx = nlp.cx.sym("target", target_shape)
weight_cx = nlp.cx.sym("weight", 1, 1)
exponent = 2 if self.quadratic and self.weight else 1
if is_trapezoidal:
# Hypothesis: the function is continuous on states
# it neglects the discontinuities at the beginning of the optimization
state_cx = (
horzcat(all_pn.nlp.states.cx, all_pn.nlp.states.cx_end)
if self.integration_rule == IntegralApproximation.TRAPEZOIDAL
else all_pn.nlp.states.cx
)
# to handle piecewise constant in controls we have to compute the value for the end of the interval
# which only relies on the value of the control at the beginning of the interval
control_cx = (
horzcat(all_pn.nlp.controls.cx)
if nlp.control_type == ControlType.CONSTANT
else horzcat(all_pn.nlp.controls.cx, all_pn.nlp.controls.cx_end)
)
control_cx_end = get_u(nlp, control_cx, dt_cx)
state_cx_end = (
all_pn.nlp.states.cx_end
if self.integration_rule == IntegralApproximation.TRAPEZOIDAL
else nlp.dynamics[0](x0=state_cx, p=control_cx_end, params=nlp.parameters.cx)["xf"]
)
self.modified_function = biorbd.to_casadi_func(
f"{name}",
(
(self.function(all_pn.nlp.states.cx, all_pn.nlp.controls.cx, param_cx) - target_cx[:, 0])
** exponent
+ (self.function(state_cx_end, control_cx_end, param_cx) - target_cx[:, 1]) ** exponent
)
/ 2,
state_cx,
control_cx,
param_cx,
target_cx,
dt_cx,
)
modified_fcn = self.modified_function(state_cx, control_cx, param_cx, target_cx, dt_cx)
else:
modified_fcn = (self.function(state_cx, control_cx, param_cx) - target_cx) ** exponent
modified_fcn = weight_cx * modified_fcn * dt_cx if self.weight else modified_fcn * dt_cx
# Do not use nlp.add_casadi_func because all of them must be registered
self.weighted_function = Function(
name, [state_cx, control_cx, param_cx, weight_cx, target_cx, dt_cx], [modified_fcn]
)
self.weighted_function_non_threaded = self.weighted_function
if ocp.n_threads > 1 and self.multi_thread and len(self.node_idx) > 1:
self.function = self.function.map(len(self.node_idx), "thread", ocp.n_threads)
self.weighted_function = self.weighted_function.map(len(self.node_idx), "thread", ocp.n_threads)
else:
self.multi_thread = False # Override the multi_threading, since only one node is optimized
if self.expand:
self.function = self.function.expand()
self.weighted_function = self.weighted_function.expand()
def add_target_to_plot(self, all_pn: PenaltyNodeList, combine_to: str):
"""
Interface to the plot so it can be properly added to the proper plot
Parameters
----------
all_pn: PenaltyNodeList
The penalty node elements
combine_to: str
The name of the underlying plot to combine the tracking data to
"""
if self.target is None or combine_to is None:
return
self.target_plot_name = combine_to
# if the target is n x ns, we need to add a dimension (n x ns + 1) to make it compatible with the plot
if self.target[0].shape[1] == all_pn.nlp.ns:
self.target_to_plot = np.concatenate(
(self.target[0], np.nan * np.ndarray((self.target[0].shape[0], 1))), axis=1
)
else:
self.target_to_plot = self.target[0]
def _finish_add_target_to_plot(self, all_pn: PenaltyNodeList):
"""
Internal interface to add (after having check the target dimensions) the target to the plot if needed
Parameters
----------
all_pn: PenaltyNodeList
The penalty node elements
"""
def plot_function(t, x, u, p):
if isinstance(t, (list, tuple)):
return self.target_to_plot[:, [self.node_idx.index(_t) for _t in t]]
else:
return self.target_to_plot[:, self.node_idx.index(t)]
if self.target_to_plot is not None:
if self.target_to_plot.shape[1] > 1:
plot_type = PlotType.STEP
else:
plot_type = PlotType.POINT
all_pn.ocp.add_plot(
self.target_plot_name,
plot_function,
color="tab:red",
plot_type=plot_type,
phase=all_pn.nlp.phase_idx,
axes_idx=Mapping(self.rows),
node_idx=self.node_idx,
)
def add_or_replace_to_penalty_pool(self, ocp, nlp):
"""
Doing some configuration on the penalty and add it to the list of penalty
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
nlp: NonLinearProgram
A reference to the current phase of the ocp
"""
if not self.name:
if self.type.name == "CUSTOM":
self.name = self.custom_function.__name__
else:
self.name = self.type.name
penalty_type = self.type.get_type()
if self.node == Node.TRANSITION:
all_pn = []
# Make sure the penalty behave like a PhaseTransition, even though it may be an Objective or Constraint
self.node = Node.END
self.node_idx = [0]
self.transition = True
self.dt = 1
self.phase_pre_idx = nlp.phase_idx
self.phase_post_idx = (nlp.phase_idx + 1) % ocp.n_phases
if not self.states_mapping:
self.states_mapping = BiMapping(range(nlp.states.shape), range(nlp.states.shape))
all_pn.append(self._get_penalty_node_list(ocp, nlp))
all_pn[0].u = [nlp.U[-1]] # Make an exception to the fact that U is not available for the last node
nlp = ocp.nlp[(nlp.phase_idx + 1) % ocp.n_phases]
self.node = Node.START
all_pn.append(self._get_penalty_node_list(ocp, nlp))
self.node = Node.TRANSITION
penalty_type.validate_penalty_time_index(self, all_pn[0])
penalty_type.validate_penalty_time_index(self, all_pn[1])
self.clear_penalty(ocp, all_pn[0].nlp)
elif isinstance(self.node, tuple) and self.multinode_constraint:
all_pn = []
self.node_list = self.node
# Make sure the penalty behave like a MultinodeConstraint, even though it may be an Objective or Constraint
# self.transition = True
self.dt = 1
# self.phase_pre_idx
# self.phase_post_idx = (nlp.phase_idx + 1) % ocp.n_phases
if not self.states_mapping:
self.states_mapping = BiMapping(range(nlp.states.shape), range(nlp.states.shape))
self.node = self.node_list[0]
nlp = ocp.nlp[self.phase_first_idx]
all_pn.append(self._get_penalty_node_list(ocp, nlp))
if self.node == Node.END:
all_pn[0].u = [nlp.U[-1]]
# Make an exception to the fact that U is not available for the last node
self.node = self.node_list[1]
nlp = ocp.nlp[self.phase_second_idx]
all_pn.append(self._get_penalty_node_list(ocp, nlp))
if self.node == Node.END:
all_pn[1].u = [nlp.U[-1]]
# Make an exception to the fact that U is not available for the last node
# reset the node list
self.node = self.node_list
penalty_type.validate_penalty_time_index(self, all_pn[0])
penalty_type.validate_penalty_time_index(self, all_pn[1])
self.node_idx = [all_pn[0].t[0], all_pn[1].t[0]]
self.clear_penalty(ocp, all_pn[0].nlp)
else:
all_pn = self._get_penalty_node_list(ocp, nlp)
penalty_type.validate_penalty_time_index(self, all_pn)
self.clear_penalty(all_pn.ocp, all_pn.nlp)
self.dt = penalty_type.get_dt(all_pn.nlp)
self.node_idx = (
all_pn.t[:-1]
if (
self.integration_rule == IntegralApproximation.TRAPEZOIDAL
or self.integration_rule == IntegralApproximation.TRUE_TRAPEZOIDAL
)
and self.target is not None
else all_pn.t
)
penalty_function = self.type.value[0](self, all_pn, **self.params)
self.set_penalty(penalty_function, all_pn)
def _add_penalty_to_pool(self, all_pn: PenaltyNodeList):
"""
Return the penalty pool for the specified penalty (abstract)
Parameters
----------
all_pn: PenaltyNodeList
The penalty node elements
"""
raise RuntimeError("get_dt cannot be called from an abstract class")
def clear_penalty(self, ocp, nlp):
"""
Resets a penalty. A negative penalty index creates a new empty penalty (abstract)
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
nlp: NonLinearProgram
A reference to the current phase of the ocp
"""
raise RuntimeError("_reset_penalty cannot be called from an abstract class")
def _get_penalty_node_list(self, ocp, nlp) -> PenaltyNodeList:
"""
Get the actual node (time, X and U) specified in the penalty
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
nlp: NonLinearProgram
A reference to the current phase of the ocp
Returns
-------
The actual node (time, X and U) specified in the penalty
"""
if not isinstance(self.node, (list, tuple)):
self.node = (self.node,)
t = []
for node in self.node:
if isinstance(node, int):
if node < 0 or node > nlp.ns:
raise RuntimeError(f"Invalid node, {node} must be between 0 and {nlp.ns}")
t.append(node)
elif node == Node.START:
t.append(0)
elif node == Node.MID:
if nlp.ns % 2 == 1:
raise (ValueError("Number of shooting points must be even to use MID"))
t.append(nlp.ns // 2)
elif node == Node.INTERMEDIATES:
t.extend(list(i for i in range(1, nlp.ns - 1)))
elif node == Node.PENULTIMATE:
if nlp.ns < 2:
raise (ValueError("Number of shooting points must be greater than 1"))
t.append(nlp.ns - 1)
elif node == Node.END:
t.append(nlp.ns)
elif node == Node.ALL_SHOOTING:
t.extend(range(nlp.ns))
elif node == Node.ALL:
t.extend(range(nlp.ns + 1))
else:
raise RuntimeError(" is not a valid node")
x = [nlp.X[idx] for idx in t]
u = [nlp.U[idx] for idx in t if idx != nlp.ns]
return PenaltyNodeList(ocp, nlp, t, x, u, nlp.parameters.cx)
def _create_nu_update_func(self):
v = self.opt_problem.x
x_var, y_var, u_var, eta, p_opt, theta_opt = self.ocp_solver.discretizer.unpack_decision_variables(
v)
par = MX.sym('par', self.model.n_p)
theta = dict([(i, vec(MX.sym('theta_' + repr(i), self.model.n_theta)))
for i in range(self.finite_elements)])
theta_var = vertcat(*[theta[i] for i in range(self.finite_elements)])
time_dict = dict([(i, {}) for i in range(self.finite_elements)])
for el in range(self.finite_elements):
time_dict[el]['t_0'] = self.time_breakpoints[el]
time_dict[el]['t_f'] = self.time_breakpoints[el + 1]
time_dict[el]['f_nu'] = self.time_interpolation_nu[el]
time_dict[el]['f_relax_alg'] = self.time_interpolation_nu[el]
time_dict[el]['f_relax_eq'] = self.time_interpolation_nu[el]
functions = defaultdict(dict)
for el in range(self.finite_elements):
func_rel_alg = self.model.convert_expr_from_tau_to_time(
substitute(self.relaxed_alg, self.model.u, self.model.u_expr),
self.time_breakpoints[el], self.time_breakpoints[el + 1])
func_rel_eq = self.model.convert_expr_from_tau_to_time(
substitute(self.relaxed_eq, self.model.u, self.model.u_expr),
self.time_breakpoints[el], self.time_breakpoints[el + 1])
nu_time_dependent = self.model.convert_expr_from_tau_to_time(
self.nu_pol, self.time_breakpoints[el],
self.time_breakpoints[el + 1])
f_nu = Function('f_nu', self.model.all_sym, [nu_time_dependent])
f_relax_alg = Function('f_relax_alg', self.model.all_sym,
[func_rel_alg])
f_relax_eq = Function('f_relax_eq', self.model.all_sym,
[func_rel_eq])
functions['f_nu'][el] = f_nu
functions['f_relax_alg'][el] = f_relax_alg
functions['f_relax_eq'][el] = f_relax_eq
# get values (symbolically)
results = self.ocp_solver.discretizer.get_system_at_given_times(
x_var,
y_var,
u_var,
time_dict,
p=par,
theta=theta,
functions=functions)
# compute new nu
mu_mx = par[find_variables_indices_in_vector(self.mu_sym,
self.model.p)]
new_nu = []
rel_alg = []
rel_eq = []
for el in range(self.finite_elements):
new_nu_k = []
rel_alg_k = []
rel_eq_k = []
for j in range(self.degree):
nu_kj = results[el]['f_nu'][j]
rel_alg_kj = results[el]['f_relax_alg'][j]
rel_eq_kj = results[el]['f_relax_eq'][j]
rel_kj = vertcat(rel_alg_kj, rel_eq_kj)
new_nu_k = horzcat(new_nu_k, nu_kj + mu_mx * rel_kj)
rel_alg_k = horzcat(rel_alg_k, rel_alg_kj)
rel_eq_k = horzcat(rel_eq_k, rel_eq_kj)
new_nu.append(vec(new_nu_k))
rel_alg.append(vec(rel_alg_k))
rel_eq.append(vec(rel_eq_k))
output = new_nu + rel_alg + rel_eq
self.new_nu_func = Function('nu_update_function', [v, par, theta_var],
output)
def _set_penalty_function(self, all_pn: Union[PenaltyNodeList, list, tuple], fcn: Union[MX, SX]):
"""
Finalize the preparation of the penalty (setting function and weighted_function)
Parameters
----------
all_pn: PenaltyNodeList
The nodes
fcn: Union[MX, SX]
The value of the penalty function
"""
# Sanity checks
if self.transition and self.explicit_derivative:
raise ValueError("transition and explicit_derivative cannot be true simultaneously")
if self.transition and self.derivative:
raise ValueError("transition and derivative cannot be true simultaneously")
if self.derivative and self.explicit_derivative:
raise ValueError("derivative and explicit_derivative cannot be true simultaneously")
def get_u(nlp, u: Union[MX, SX], dt: Union[MX, SX]):
"""
Get the control at a given time
Parameters
----------
nlp: NonlinearProgram
The nonlinear program
u: Union[MX, SX]
The control matrix
dt: Union[MX, SX]
The time a which control should be computed
Returns
-------
The control at a given time
"""
if nlp.control_type == ControlType.CONSTANT:
return u
elif nlp.control_type == ControlType.LINEAR_CONTINUOUS:
return u[:, 0] + (u[:, 1] - u[:, 0]) * dt
else:
raise RuntimeError(f"{nlp.control_type} ControlType not implemented yet")
return u
if self.multinode_constraint or self.transition:
ocp = all_pn[0].ocp
nlp = all_pn[0].nlp
nlp_post = all_pn[1].nlp
name = self.name.replace("->", "_").replace(" ", "_").replace(",", "_")
states_pre = nlp.states.cx_end
states_post = nlp_post.states.cx
controls_pre = nlp.controls.cx_end
controls_post = nlp_post.controls.cx
state_cx = vertcat(states_pre, states_post)
control_cx = vertcat(controls_pre, controls_post)
else:
ocp = all_pn.ocp
nlp = all_pn.nlp
name = self.name
if self.integrate:
state_cx = horzcat(*([all_pn.nlp.states.cx] + all_pn.nlp.states.cx_intermediates_list))
control_cx = all_pn.nlp.controls.cx
else:
state_cx = all_pn.nlp.states.cx
control_cx = all_pn.nlp.controls.cx
if self.explicit_derivative:
if self.derivative:
raise RuntimeError("derivative and explicit_derivative cannot be simultaneously true")
state_cx = horzcat(state_cx, all_pn.nlp.states.cx_end)
control_cx = horzcat(control_cx, all_pn.nlp.controls.cx_end)
param_cx = nlp.cx(nlp.parameters.cx)
# Do not use nlp.add_casadi_func because all functions must be registered
sub_fcn = fcn[self.rows, self.cols]
self.function = biorbd.to_casadi_func(name, sub_fcn, state_cx, control_cx, param_cx, expand=self.expand)
self.function_non_threaded = self.function
if self.derivative:
state_cx = horzcat(all_pn.nlp.states.cx_end, all_pn.nlp.states.cx)
control_cx = horzcat(all_pn.nlp.controls.cx_end, all_pn.nlp.controls.cx)
self.function = biorbd.to_casadi_func(
f"{name}",
self.function(all_pn.nlp.states.cx_end, all_pn.nlp.controls.cx_end, param_cx)
- self.function(all_pn.nlp.states.cx, all_pn.nlp.controls.cx, param_cx),
state_cx,
control_cx,
param_cx,
)
dt_cx = nlp.cx.sym("dt", 1, 1)
is_trapezoidal = (
self.integration_rule == IntegralApproximation.TRAPEZOIDAL
or self.integration_rule == IntegralApproximation.TRUE_TRAPEZOIDAL
)
target_shape = tuple(
[
len(self.rows),
len(self.cols) + 1 if is_trapezoidal else len(self.cols),
]
)
target_cx = nlp.cx.sym("target", target_shape)
weight_cx = nlp.cx.sym("weight", 1, 1)
exponent = 2 if self.quadratic and self.weight else 1
if is_trapezoidal:
# Hypothesis: the function is continuous on states
# it neglects the discontinuities at the beginning of the optimization
state_cx = (
horzcat(all_pn.nlp.states.cx, all_pn.nlp.states.cx_end)
if self.integration_rule == IntegralApproximation.TRAPEZOIDAL
else all_pn.nlp.states.cx
)
# to handle piecewise constant in controls we have to compute the value for the end of the interval
# which only relies on the value of the control at the beginning of the interval
control_cx = (
horzcat(all_pn.nlp.controls.cx)
if nlp.control_type == ControlType.CONSTANT
else horzcat(all_pn.nlp.controls.cx, all_pn.nlp.controls.cx_end)
)
control_cx_end = get_u(nlp, control_cx, dt_cx)
state_cx_end = (
all_pn.nlp.states.cx_end
if self.integration_rule == IntegralApproximation.TRAPEZOIDAL
else nlp.dynamics[0](x0=state_cx, p=control_cx_end, params=nlp.parameters.cx)["xf"]
)
self.modified_function = biorbd.to_casadi_func(
f"{name}",
(
(self.function(all_pn.nlp.states.cx, all_pn.nlp.controls.cx, param_cx) - target_cx[:, 0])
** exponent
+ (self.function(state_cx_end, control_cx_end, param_cx) - target_cx[:, 1]) ** exponent
)
/ 2,
state_cx,
control_cx,
param_cx,
target_cx,
dt_cx,
)
modified_fcn = self.modified_function(state_cx, control_cx, param_cx, target_cx, dt_cx)
else:
modified_fcn = (self.function(state_cx, control_cx, param_cx) - target_cx) ** exponent
modified_fcn = weight_cx * modified_fcn * dt_cx if self.weight else modified_fcn * dt_cx
# Do not use nlp.add_casadi_func because all of them must be registered
self.weighted_function = Function(
name, [state_cx, control_cx, param_cx, weight_cx, target_cx, dt_cx], [modified_fcn]
)
self.weighted_function_non_threaded = self.weighted_function
if ocp.n_threads > 1 and self.multi_thread and len(self.node_idx) > 1:
self.function = self.function.map(len(self.node_idx), "thread", ocp.n_threads)
self.weighted_function = self.weighted_function.map(len(self.node_idx), "thread", ocp.n_threads)
else:
self.multi_thread = False # Override the multi_threading, since only one node is optimized
if self.expand:
self.function = self.function.expand()
self.weighted_function = self.weighted_function.expand()
q_dot = states_sol["q_dot"]
activations = states_sol["muscles"]
tau = controls_sol["tau"]
excitations = controls_sol["muscles"]
n_q = ocp.nlp[0]["model"].nbQ()
n_qdot = ocp.nlp[0]["model"].nbQdot()
n_mark = ocp.nlp[0]["model"].nbMarkers()
n_frames = q.shape[1]
markers = np.ndarray((3, n_mark, q.shape[1]))
symbolic_states = MX.sym("x", n_q, 1)
markers_func = Function(
"ForwardKin",
[symbolic_states],
[biorbd_model.markers(symbolic_states)],
["q"],
["markers"],
).expand()
for i in range(n_frames):
markers[:, :, i] = markers_func(q[:, i])
plt.figure("Markers")
for i in range(markers.shape[1]):
plt.plot(np.linspace(0, 2, n_shooting_points + 1),
markers_ref[:, i, :].T, "k")
plt.plot(np.linspace(0, 2, n_shooting_points + 1), markers[:, i, :].T,
"r--")
plt.xlabel("Time")
plt.ylabel("Markers Position")
def qp_solve(prob, obj, p_init, x_init, y_init, lam_opt, mu_opt, case):
"""
QP solver for path-following algorithm
inputs: prob - problem description
obj - problem equations
p_init - initial parameter
x_init - initial primal variable
y_init - initial dual variable
lam_opt - Lagrange multipliers of equality and active constraints
mu_opt - Lagrange multipliers of inequality constraints
outputs: y - solution primal variable
qp_val - objective function value
qp_exit - return status of QP solver
deriv - derivatives of the problem
k_zero_tilde - active set index
k_plus_tilde - inactive set index
grad - gradient of objective function
"""
print 'Current point x:', x_init
#Importing problem to be solved
nx, np, neq, niq, name = prob()
x, p, f, f_fun, con, conf, ubx, lbx, ubg, lbg = obj(
x_init, y_init, p_init, neq, niq, nx, np)
#Deteriming constraint types
eq_con_ind = array([]) #indices of equality constraints
iq_con_ind = array([]) #indices of inequality constraints
eq_con = array([]) #equality constraints
iq_con = array([]) #inequality constraints
for i in range(0, len(lbg[0])):
if lbg[0, i] == 0:
eq_con = vertcat(eq_con, con[i])
eq_con_ind = append(eq_con_ind, i)
elif lbg[0, i] < 0:
iq_con = vertcat(iq_con, con[i])
iq_con_ind = append(iq_con_ind, i)
# print 'Equality Constraint:', eq_con
# print 'Inequality Constraint:', iq_con
# if case == 'pure-predictor':
# return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
if case == 'predictor-corrector':
#Evaluating constraints at current iteration point
con_vals = conf(x_init, p_init)
#Determining which inequality constraints are active
k_plus_tilde = array([]) #active constraint
k_zero_tilde = array([]) #inactive constraint
tol = 10e-5 #tolerance
for i in range(0, len(iq_con_ind)):
if ubg[0, i] - tol <= con_vals[i] and con_vals[i] <= ubg[0,
i] + tol:
k_plus_tilde = append(k_plus_tilde, i)
else:
k_zero_tilde = append(k_zero_tilde, i)
# print 'Active constraints:', k_plus_tilde
# print 'Inactive constraints:', k_zero_tilde
# print 'Constraint values:', con_vals
nk_pt = len(k_plus_tilde) #number of active constraints
nk_zt = len(k_zero_tilde) #number of inactive constraints
#Calculating Lagrangian
lam = SX.sym('lam', neq) #Lagrangian multiplier equality constraints
mu = SX.sym('mu', niq) #Lagrangian multiplier inequality constraints
lag_f = f + mtimes(lam.T, eq_con) + mtimes(
mu.T, iq_con) #Lagrangian equation
#Calculating derivatives
g = gradient(f, x) #Derivative of objective function
g_fun = Function('g_fun', [x, p], [gradient(f, x)])
H = 2 * jacobian(gradient(lag_f, x),
x) #Second derivative of the Lagrangian
H_fun = Function('H_fun', [x, p, lam, mu],
[jacobian(jacobian(lag_f, x), x)])
if len(eq_con_ind) > 0:
deq = jacobian(eq_con, x) #Derivative of equality constraints
else:
deq = array([])
if len(iq_con_ind) > 0:
diq = jacobian(iq_con, x) #Derivative of inequality constraints
else:
diq = array([])
#Creating constraint matrices
nc = niq + neq #Total number of constraints
if (niq > 0) and (neq > 0): #Equality and inequality constraints
#this part needs to be tested
if (nk_zt > 0): #Inactive constraints exist
A = SX.zeros((nc, nx))
print deq
A[0, :] = deq #A matrix
lba = -1e16 * SX.zeros((nc, 1))
lba[0, :] = -eq_con #lower bound of A
uba = 1e16 * SX.zeros((nc, 1))
uba[0, :] = -eq_con #upper bound of A
for j in range(0, nk_pt): #adding active constraints
A[neq + j + 1, :] = diq[int(k_plus_tilde[j]), :]
lba[neq + j + 1] = -iq_con[int(k_plus_tilde[j])]
uba[neq + j + 1] = -iq_con[int(k_plus_tilde[i])]
for i in range(0, nk_zt): #adding inactive constraints
A[neq + nk_pt + i + 1, :] = diq[int(k_zero_tilde[i]), :]
uba[neq + nk_pt + i + 1] = -iq_con[int(k_zero_tilde[i])]
#inactive constraints don't have lower bounds
else: #Active constraints only
A = vertcat(deq, diq)
lba = vertcat(-eq_con, -iq_con)
uba = vertcat(-eq_con, -iq_con)
elif (niq > 0) and (neq == 0): #Inquality constraints
if (nk_zt > 0): #Inactive constraints exist
A = SX.zeros((nc, nx))
lba = -1e16 * SX.ones((nc, 1))
uba = 1e16 * SX.ones((nc, 1))
for j in range(0, nk_pt): #adding active constraints
A[j, :] = diq[int(k_plus_tilde[j]), :]
lba[j] = -iq_con[int(k_plus_tilde[j])]
uba[j] = -iq_con[int(k_plus_tilde[j])]
for i in range(0, nk_zt): #adding inactive constraints
A[nk_pt + i, :] = diq[int(k_zero_tilde[i]), :]
uba[nk_pt + i] = -iq_con[int(k_zero_tilde[i])]
#inactive constraints don't have lower bounds
else:
raw_input()
A = vertcat(deq, diq)
lba = -iq_con
uba = -iq_con
elif (niq == 0) and (neq > 0): #Equality constriants
A = deq
lba = -eq_con
uba = -eq_con
A_fun = Function('A_fun', [x, p], [A])
lba_fun = Function('lba_fun', [x, p], [lba])
uba_fun = Function('uba_fun', [x, p], [uba])
#Checking that matrices are correct sizes and types
if (H.size1() != nx) or (H.size2() != nx) or (H.is_dense() == 'False'):
#H matrix should be a sparse (nxn) and symmetrical
print(
'WARNING: H matrix is not the correct dimensions or matrix type'
)
if (g.size1() != nx) or (g.size2() != 1) or g.is_dense() == 'True':
#g matrix should be a dense (nx1)
print(
'WARNING: g matrix is not the correct dimensions or matrix type'
)
if (A.size1() !=
(neq + niq)) or (A.size2() != nx) or (A.is_dense() == 'False'):
#A should be a sparse (nc x n)
print(
'WARNING: A matrix is not the correct dimensions or matrix type'
)
if lba.size1() != (neq + niq) or (lba.size2() !=
1) or lba.is_dense() == 'False':
print(
'WARNING: lba matrix is not the correct dimensions or matrix type'
)
if uba.size1() != (neq + niq) or (uba.size2() !=
1) or uba.is_dense() == 'False':
print(
'WARNING: uba matrix is not the correct dimensions or matrix type'
)
#Evaluating QP matrices at optimal points
H_opt = H_fun(x_init, p_init, lam_opt, mu_opt)
g_opt = g_fun(x_init, p_init)
A_opt = A_fun(x_init, p_init)
lba_opt = lba_fun(x_init, p_init)
uba_opt = uba_fun(x_init, p_init)
# print 'Lower bounds', lba_opt
# print 'Upper bounds', uba_opt
# print 'Bound matrix', A_opt
#Defining QP structure
qp = {}
qp['h'] = H_opt.sparsity()
qp['a'] = A_opt.sparsity()
optimize = conic('optimize', 'qpoases', qp)
optimal = optimize(h=H_opt,
g=g_opt,
a=A_opt,
lba=lba_opt,
uba=uba_opt,
x0=x_init)
x_qpopt = optimal['x']
if x_qpopt.shape == x_init.shape:
qp_exit = 'optimal'
else:
qp_exit = ''
lag_qpopt = optimal['lam_a']
#Determing Lagrangian multipliers (lambda and mu)
lam_qpopt = zeros(
(nk_pt, 1)) #Lagrange multiplier of active constraints
mu_qpopt = zeros(
(nk_zt, 1)) #Lagrange multiplier of inactive constraints
if nk_pt > 0:
for j in range(0, len(k_plus_tilde)):
lam_qpopt[j] = lag_qpopt[int(k_plus_tilde[j])]
if nk_zt > 0:
for k in range(0, len(k_zero_tilde)):
print lag_qpopt[int(k_zero_tilde[k])]
return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
