def online_muscle_torque(ocp: OptimalControlProgram):
nlp = ocp.nlp[0]
states = MX.sym("x", nlp.nx, 1)
controls = MX.sym("u", nlp.nu, 1)
parameters = MX.sym("u", nlp.np, 1)
nq = nlp.mapping["q"].to_first.len
q = nlp.mapping["q"].to_second.map(states[:nq])
qdot = nlp.mapping["qdot"].to_second.map(states[nq:])
muscles_states = nlp.model.stateSet()
muscles_activations = controls[nlp.shape["tau"]:]
for k in range(nlp.shape["muscle"]):
muscles_states[k].setActivation(muscles_activations[k])
muscles_tau = nlp.model.muscularJointTorque(muscles_states, q,
qdot).to_mx()
muscle_tau_func = Function("muscle_tau", [states, controls, parameters],
[muscles_tau]).expand()
def muscle_tau_callback(s, c, p):
return muscle_tau_func(s, c, p)
ocp.add_plot(
"muscle_torque",
muscle_tau_callback,
plot_type=PlotType.STEP,
)
def configure_dynamics_function(ocp, nlp, dyn_func, **extra_params):
"""
Configure the forward dynamics
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
nlp: NonLinearProgram
A reference to the phase
dyn_func: Callable[states, controls, param]
The function to get the derivative of the states
"""
mx_symbolic_states = MX.sym("x", nlp.states.shape, 1)
mx_symbolic_controls = MX.sym("u", nlp.controls.shape, 1)
nlp.parameters = ocp.v.parameters_in_list
mx_symbolic_params = MX.sym("p", nlp.parameters.shape, 1)
dynamics = dyn_func(mx_symbolic_states, mx_symbolic_controls, mx_symbolic_params, nlp, **extra_params)
if isinstance(dynamics, (list, tuple)):
dynamics = vertcat(*dynamics)
nlp.dynamics_func = Function(
"ForwardDyn",
[mx_symbolic_states, mx_symbolic_controls, mx_symbolic_params],
[dynamics],
["x", "u", "p"],
["xdot"],
).expand()
def configure_forward_dyn_func(ocp, nlp, dyn_func):
nlp.nx = nlp.x.rows()
nlp.nu = nlp.u.rows()
MX_symbolic_states = MX.sym("x", nlp.nx, 1)
MX_symbolic_controls = MX.sym("u", nlp.nu, 1)
symbolic_params = nlp.CX()
nlp.parameters_to_optimize = ocp.param_to_optimize
for key in nlp.parameters_to_optimize:
symbolic_params = vertcat(symbolic_params,
nlp.parameters_to_optimize[key]["cx"])
nlp.p = symbolic_params
nlp.np = symbolic_params.rows()
MX_symbolic_params = MX.sym("p", nlp.np, 1)
dynamics = dyn_func(MX_symbolic_states, MX_symbolic_controls,
MX_symbolic_params, nlp)
if isinstance(dynamics, (list, tuple)):
dynamics = vertcat(*dynamics)
nlp.dynamics_func = Function(
"ForwardDyn",
[MX_symbolic_states, MX_symbolic_controls, MX_symbolic_params],
[dynamics],
["x", "u", "p"],
["xdot"],
).expand()
def __init__(self,
dt=0.01,
I=np.diag(np.array([2, 2, 4])),
kd=1,
k=1,
L=0.3,
b=1,
m=1,
g=9.81,
Dx=12,
Du=4):
self.I = I #inertia
self.I_inv = np.linalg.inv(self.I)
self.kd = kd #friction
self.k = k #motor constant
self.L = L # distance between center and motor
self.b = b # drag coefficient
self.m = m # mass
self.g = g
self.Dx = Dx
self.Du = Du
self.dt = dt
self.gravity = np.array([0, 0, -self.g])
self.x = MX.sym('x', Dx)
self.u = MX.sym('u', Du)
#initialise
self.def_step_func(self.x, self.u)
self.def_jacobian()
def configure_forward_dyn_func(ocp, nlp, dyn_func):
nlp["nx"] = nlp["x"].rows()
nlp["nu"] = nlp["u"].rows()
MX_symbolic_states = MX.sym("x", nlp["nx"], 1)
MX_symbolic_controls = MX.sym("u", nlp["nu"], 1)
symbolic_params = nlp["CX"]()
nlp["parameters_to_optimize"] = ocp.param_to_optimize
for key in nlp["parameters_to_optimize"]:
symbolic_params = vertcat(symbolic_params,
nlp["parameters_to_optimize"][key]["cx"])
nlp["p"] = symbolic_params
nlp["np"] = symbolic_params.rows()
MX_symbolic_params = MX.sym("p", nlp["np"], 1)
dynamics = dyn_func(MX_symbolic_states, MX_symbolic_controls,
MX_symbolic_params, nlp)
if isinstance(dynamics, (list, tuple)):
dynamics = vertcat(*dynamics)
nlp["dynamics_func"] = Function(
"ForwardDyn",
[MX_symbolic_states, MX_symbolic_controls, MX_symbolic_params],
[dynamics],
["x", "u", "p"],
["xdot"],
).expand()
def configure_contact(ocp, nlp, dyn_func):
symbolic_states = MX.sym("x", nlp.nx, 1)
symbolic_controls = MX.sym("u", nlp.nu, 1)
symbolic_param = MX.sym("p", nlp.np, 1)
nlp.contact_forces_func = Function(
"contact_forces_func",
[symbolic_states, symbolic_controls, symbolic_param],
[dyn_func(symbolic_states, symbolic_controls, symbolic_param, nlp)],
["x", "u", "p"],
["contact_forces"],
).expand()
all_contact_names = []
for elt in ocp.nlp:
all_contact_names.extend(
[name.to_string() for name in elt.model.contactNames() if name.to_string() not in all_contact_names]
)
if "contact_forces" in nlp.mapping["plot"]:
phase_mappings = nlp.mapping["plot"]["contact_forces"]
else:
contact_names_in_phase = [name.to_string() for name in nlp.model.contactNames()]
phase_mappings = Mapping([i for i, c in enumerate(all_contact_names) if c in contact_names_in_phase])
nlp.plot["contact_forces"] = CustomPlot(
nlp.contact_forces_func, axes_idx=phase_mappings, legend=all_contact_names
)
def test_markers(brbd):
m = brbd.Model("../../models/pyomecaman.bioMod")
q = np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3])
q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3])
expected_markers_last = np.array([-0.11369, 0.63240501, -0.56253268])
expected_markers_last_dot = np.array([0.0, 4.16996219, 3.99459262])
if brbd.currentLinearAlgebraBackend() == 1:
# If CasADi backend is used
from casadi import MX
q_sym = MX.sym("q", m.nbQ(), 1)
q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1)
markers_func = brbd.to_casadi_func("Compute_Markers", m.markers, q_sym)
markers_velocity_func = brbd.to_casadi_func("Compute_MarkersVelocity", m.markersVelocity, q_sym, q_dot_sym)
markers = np.array(markers_func(q))
markers_dot = np.array(markers_velocity_func(q, q_dot))
elif not brbd.currentLinearAlgebraBackend():
# If Eigen backend is used
markers = np.array([mark.to_array() for mark in m.markers(q)]).T
markers_dot = np.array([mark.to_array() for mark in m.markersVelocity(q, q_dot)]).T
else:
raise NotImplementedError("Backend not implemented in test")
np.testing.assert_almost_equal(markers[:, -1], expected_markers_last)
np.testing.assert_almost_equal(markers_dot[:, -1], expected_markers_last_dot)
def test_blockdiag(self):
# Test blockdiag with DM
correct_res = DM([[1, 1, 0, 0, 0], [1, 1, 0, 0, 0], [0, 0, 1, 1, 1],
[0, 0, 1, 1, 1], [0, 0, 1, 1, 1]])
a = DM.ones(2, 2)
b = DM.ones(3, 3)
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res))
# MX and DM mix
a = MX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = MX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and DM mix
a = SX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = SX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and MX
a = SX.sym('a', 2, 2)
b = MX.sym('b', 2, 2)
self.assertRaises(ValueError, blockdiag, a, b)
def muscle_excitations_and_torque_driven_with_contact(nlp):
"""
Names states (nlp.x) and controls (nlp.u) and gives size to (nlp.nx) and (nlp.nu).
Works with torques and muscles.
:param nlp: An OptimalControlProgram class.
"""
ProblemType.__configure_q_qdot(nlp, True, False)
ProblemType.__configure_tau(nlp, False, True)
ProblemType.__configure_muscles(nlp, True, True)
u = MX()
x = MX()
for i in range(nlp["nbMuscle"]):
u = vertcat(u, MX.sym(f"Muscle_{nlp['muscleNames']}_excitation"))
x = vertcat(x, MX.sym(f"Muscle_{nlp['muscleNames']}_activation"))
nlp["u"] = vertcat(nlp["u"], u)
nlp["x"] = vertcat(nlp["x"], x)
nlp["var_states"]["muscles"] = nlp["nbMuscle"]
nlp["var_controls"]["muscles"] = nlp["nbMuscle"]
ProblemType.__configure_forward_dyn_func(
nlp, Dynamics.
forward_dynamics_muscle_excitations_and_torque_driven_with_contact)
ProblemType.__configure_contact(
nlp, Dynamics.
forces_from_forward_dynamics_muscle_excitations_and_torque_driven_with_contact
)
def export_external_ode_model():
model_name = 'external_ode'
# Declare model variables
x = MX.sym('x', 2)
u = MX.sym('u', 1)
xDot = MX.sym('xDot', 2)
cdll.LoadLibrary('./test_external_lib/build/libexternal_ode_casadi.so')
f_ext = external('libexternal_ode_casadi', 'libexternal_ode_casadi.so',
{'enable_fd': True})
f_expl = f_ext(x, u)
f_impl = xDot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xDot
model.u = u
model.p = []
model.name = model_name
return model
def test_CoM():
m = biorbd.Model("../../models/pyomecaman.bioMod")
q = np.array(
[0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3])
q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3])
q_ddot = np.array([10, 10, 10, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30])
expected_CoM = np.array(
[-0.0034679564024098523, 0.15680579877453169, 0.07808112642459612])
expected_CoM_dot = np.array(
[-0.05018973433722229, 1.4166208451420528, 1.4301750486035787])
expected_CoM_ddot = np.array(
[-0.7606169667295027, 11.508107073695976, 16.58853835505851])
if biorbd.currentLinearAlgebraBackend() == 1:
# If CasADi backend is used
from casadi import Function, MX
q_sym = MX.sym("q", m.nbQ(), 1)
q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1)
q_ddot_sym = MX.sym("q_ddot", m.nbQddot(), 1)
CoM_func = Function(
"Compute_CoM",
[q_sym],
[m.CoM(q_sym).to_mx()],
["q"],
["CoM"],
).expand()
CoM_dot_func = Function(
"Compute_CoM_dot",
[q_sym, q_dot_sym],
[m.CoMdot(q_sym, q_dot_sym).to_mx()],
["q", "q_dot"],
["CoM_dot"],
).expand()
CoM_ddot_func = Function(
"Compute_CoM_ddot",
[q_sym, q_dot_sym, q_ddot_sym],
[m.CoMddot(q_sym, q_dot_sym, q_ddot_sym).to_mx()],
["q", "q_dot", "q_ddot"],
["CoM_ddot"],
).expand()
CoM = np.array(CoM_func(q))
CoM_dot = np.array(CoM_dot_func(q, q_dot))
CoM_ddot = np.array(CoM_ddot_func(q, q_dot, q_ddot))
elif not biorbd.currentLinearAlgebraBackend():
# If Eigen backend is used
CoM = m.CoM(q).to_array()
CoM_dot = m.CoMdot(q, q_dot).to_array()
CoM_ddot = m.CoMddot(q, q_dot, q_ddot).to_array()
np.testing.assert_almost_equal(CoM.squeeze(), expected_CoM)
np.testing.assert_almost_equal(CoM_dot.squeeze(), expected_CoM_dot)
np.testing.assert_almost_equal(CoM_ddot.squeeze(), expected_CoM_ddot)
def define_substitute(self, name, expr):
if isinstance(expr, list):
return [
self.define_substitute(name + str(l), e)
for l, e in enumerate(expr)
]
else:
if name in self._substitutes:
raise ValueError('Name %s already used for substitutes!' %
(name))
symbol_name = self._add_label(name)
if isinstance(expr, BSpline):
self._splines_prim[name] = {'basis': expr.basis}
coeffs = MX.sym(symbol_name, expr.coeffs.shape[0], 1)
subst = BSpline(expr.basis, coeffs)
self._substitutes[name] = [expr.coeffs, subst.coeffs]
inp_sym, inp_num = [], []
for sym in symvar(expr.coeffs):
inp_sym.append(sym)
inp_num.append(
DM(self._values[self.symbol_dict[sym.name()][1]]))
fun = Function('eval', inp_sym, [expr.coeffs])
self._values[name] = fun(*inp_num)
self.add_to_dict(coeffs, name)
else:
subst = MX.sym(symbol_name, expr.shape[0], expr.shape[1])
self._substitutes[name] = [expr, subst]
self.add_to_dict(subst, name)
return subst
def configure_dynamics_function(ocp, nlp, dyn_func):
"""
Configure the forward dynamics
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
nlp: NonLinearProgram
A reference to the phase
dyn_func: Callable[states, controls, param]
The function to get the derivative of the states
"""
nlp.nx = nlp.x.rows()
nlp.nu = nlp.u.rows()
mx_symbolic_states = MX.sym("x", nlp.nx, 1)
mx_symbolic_controls = MX.sym("u", nlp.nu, 1)
nlp.parameters = ocp.v.parameters_in_list
nlp.p = ocp.v.parameters.cx
nlp.np = sum([p.size for p in nlp.parameters])
mx_symbolic_params = MX.sym("p", nlp.np, 1)
dynamics = dyn_func(mx_symbolic_states, mx_symbolic_controls,
mx_symbolic_params, nlp)
if isinstance(dynamics, (list, tuple)):
dynamics = vertcat(*dynamics)
nlp.dynamics_func = Function(
"ForwardDyn",
[mx_symbolic_states, mx_symbolic_controls, mx_symbolic_params],
[dynamics],
["x", "u", "p"],
["xdot"],
).expand()
def make_model_consistent(model):
x = model.x
xdot = model.xdot
u = model.u
z = model.z
p = model.p
if isinstance(x, SX):
is_SX = True
elif isinstance(x, MX):
is_SX = False
else:
raise Exception(
"model.x must be casadi.SX or casadi.MX, got {}".format(type(x)))
if is_empty(p):
if is_SX:
model.p = SX.sym('p', 0, 0)
else:
model.p = MX.sym('p', 0, 0)
if is_empty(z):
if is_SX:
model.z = SX.sym('z', 0, 0)
else:
model.z = MX.sym('z', 0, 0)
return model
def configure_q_qdot(nlp, as_states, as_controls):
"""
Configures common settings for torque driven problems with and without contacts.
:param nlp: An OptimalControlProgram class.
"""
if nlp["q_mapping"] is None:
nlp["q_mapping"] = BidirectionalMapping(
Mapping(range(nlp["model"].nbQ())), Mapping(range(nlp["model"].nbQ()))
)
if nlp["q_dot_mapping"] is None:
nlp["q_dot_mapping"] = BidirectionalMapping(
Mapping(range(nlp["model"].nbQdot())), Mapping(range(nlp["model"].nbQdot()))
)
dof_names = nlp["model"].nameDof()
q_mx = MX()
q_dot_mx = MX()
q = nlp["CX"]()
q_dot = nlp["CX"]()
for i in nlp["q_mapping"].reduce.map_idx:
q = vertcat(q, nlp["CX"].sym("Q_" + dof_names[i].to_string(), 1, 1))
for i in nlp["q_dot_mapping"].reduce.map_idx:
q_dot = vertcat(q_dot, nlp["CX"].sym("Qdot_" + dof_names[i].to_string(), 1, 1))
for i in nlp["q_mapping"].expand.map_idx:
q_mx = vertcat(q_mx, MX.sym("Q_" + dof_names[i].to_string(), 1, 1))
for i in nlp["q_dot_mapping"].expand.map_idx:
q_dot_mx = vertcat(q_dot_mx, MX.sym("Qdot_" + dof_names[i].to_string(), 1, 1))
nlp["nbQ"] = nlp["q_mapping"].reduce.len
nlp["nbQdot"] = nlp["q_dot_mapping"].reduce.len
legend_q = ["q_" + nlp["model"].nameDof()[idx].to_string() for idx in nlp["q_mapping"].reduce.map_idx]
legend_qdot = ["qdot_" + nlp["model"].nameDof()[idx].to_string() for idx in nlp["q_dot_mapping"].reduce.map_idx]
nlp["q"] = q_mx
nlp["qdot"] = q_dot_mx
if as_states:
nlp["x"] = vertcat(nlp["x"], q, q_dot)
nlp["var_states"]["q"] = nlp["nbQ"]
nlp["var_states"]["q_dot"] = nlp["nbQdot"]
nlp["plot"]["q"] = CustomPlot(
lambda x, u, p: x[: nlp["nbQ"]], plot_type=PlotType.INTEGRATED, legend=legend_q
)
nlp["plot"]["q_dot"] = CustomPlot(
lambda x, u, p: x[nlp["nbQ"] : nlp["nbQ"] + nlp["nbQdot"]],
plot_type=PlotType.INTEGRATED,
legend=legend_qdot,
)
if as_controls:
nlp["u"] = vertcat(nlp["u"], q, q_dot)
nlp["var_controls"]["q"] = nlp["nbQ"]
nlp["var_controls"]["q_dot"] = nlp["nbQdot"]
# Add plot if it happens
nlp["nx"] = nlp["x"].rows()
nlp["nu"] = nlp["u"].rows()
def test_forward_dynamics_with_external_forces():
m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod")
q = np.array([i * 1.1 for i in range(m.nbQ())])
qdot = np.array([i * 1.1 for i in range(m.nbQ())])
tau = np.array([i * 1.1 for i in range(m.nbQ())])
# With external forces
if biorbd.currentLinearAlgebraBackend() == 1:
from casadi import Function, MX
sv1 = MX((11.1, 22.2, 33.3, 44.4, 55.5, 66.6))
sv2 = MX((11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2))
else:
sv1 = np.array((11.1, 22.2, 33.3, 44.4, 55.5, 66.6))
sv2 = np.array(
(11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2))
f_ext = biorbd.VecBiorbdSpatialVector([
biorbd.SpatialVector(sv1),
biorbd.SpatialVector(sv2),
])
if biorbd.currentLinearAlgebraBackend() == 1:
q_sym = MX.sym("q", m.nbQ(), 1)
qdot_sym = MX.sym("qdot", m.nbQdot(), 1)
tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1)
ForwardDynamics = Function(
"ForwardDynamics",
[q_sym, qdot_sym, tau_sym],
[m.ForwardDynamics(q_sym, qdot_sym, tau_sym, f_ext).to_mx()],
["q_sym", "qdot_sym", "tau_sym"],
["qddot"],
).expand()
qddot = ForwardDynamics(q, qdot, tau)
qddot = np.array(qddot)[:, 0]
elif biorbd.currentLinearAlgebraBackend() == 0:
# if Eigen backend is used
qddot = m.ForwardDynamics(q, qdot, tau, f_ext).to_array()
qddot_expected = np.array([
8.8871711208009998,
-13.647827029817943,
-33.606145294752132,
16.922669487341341,
-21.882821189868423,
41.15364990805439,
68.892537246574463,
-324.59756885799197,
-447.99217990207387,
18884.241415786601,
-331.24622725851572,
1364.7620674666462,
3948.4748602722384,
])
np.testing.assert_almost_equal(qddot, qddot_expected)
def xia_model_configuration(ocp, nlp):
Problem.configure_q_qdot(nlp, True, False)
Problem.configure_tau(nlp, False, True)
Problem.configure_muscles(nlp, False, True)
x = MX()
for i in range(nlp["nbMuscle"]):
x = vertcat(
x,
MX.sym(f"Muscle_{nlp['muscleNames']}_active_{nlp['phase_idx']}", 1,
1))
for i in range(nlp["nbMuscle"]):
x = vertcat(
x,
MX.sym(f"Muscle_{nlp['muscleNames']}_fatigue_{nlp['phase_idx']}",
1, 1))
for i in range(nlp["nbMuscle"]):
x = vertcat(
x,
MX.sym(f"Muscle_{nlp['muscleNames']}_resting_{nlp['phase_idx']}",
1, 1))
nlp["x"] = vertcat(nlp["x"], x)
nlp["var_states"]["muscles_active"] = nlp["nbMuscle"]
nlp["var_states"]["muscles_fatigue"] = nlp["nbMuscle"]
nlp["var_states"]["muscles_resting"] = nlp["nbMuscle"]
nlp["nx"] = nlp["x"].rows()
nb_q_qdot = nlp["nbQ"] + nlp["nbQdot"]
nlp["plot"]["muscles_active"] = CustomPlot(
lambda x, u, p: x[nb_q_qdot:nb_q_qdot + nlp["nbMuscle"]],
plot_type=PlotType.INTEGRATED,
legend=nlp["muscleNames"],
color="r",
ylim=[0, 1],
)
combine = "muscles_active"
nlp["plot"]["muscles_fatigue"] = CustomPlot(
lambda x, u, p: x[nb_q_qdot + nlp["nbMuscle"]:nb_q_qdot + 2 * nlp[
"nbMuscle"]],
plot_type=PlotType.INTEGRATED,
legend=nlp["muscleNames"],
combine_to=combine,
color="g",
ylim=[0, 1],
)
nlp["plot"]["muscles_resting"] = CustomPlot(
lambda x, u, p: x[nb_q_qdot + 2 * nlp["nbMuscle"]:nb_q_qdot + 3 * nlp[
"nbMuscle"]],
plot_type=PlotType.INTEGRATED,
legend=nlp["muscleNames"],
combine_to=combine,
color="b",
ylim=[0, 1],
)
Problem.configure_forward_dyn_func(ocp, nlp, xia_model_dynamic)
def test_include_parameter_twice(self):
# Test to avoid override
aop = AbstractOptimizationProblem()
p1 = MX.sym('p1', 3)
p2 = MX.sym('p2', 5)
aop.include_parameter(p1)
aop.include_parameter(p2)
self.assertTrue(is_equal(aop.p[:p1.numel()], p1))
self.assertTrue(is_equal(aop.p[p1.numel():], p2))
def test_include_variable_twice(self):
# Test to avoid override
aop = AbstractOptimizationProblem()
x = MX.sym('x', 3)
y = MX.sym('y', 5)
aop.include_variable(x)
aop.include_variable(y)
self.assertTrue(is_equal(aop.x[:x.numel()], x))
self.assertTrue(is_equal(aop.x[x.numel():], y))
def test_multistep_reachability(before_test_onestep_reachability):
""" """
p, _, gp, k_fb, _, L_mu, L_sigm, c_safety, a, b = before_test_onestep_reachability
T = 3
n_u, n_s = np.shape(k_fb)
u_0 = .2 * np.random.randn(n_u, 1)
k_fb_0 = np.random.randn(
T - 1,
n_s * n_u) # np.zeros((T-1,n_s*n_u))# np.random.randn(T-1,n_s*n_u)
k_ff = np.random.randn(T - 1, n_u)
# k_fb_ctrl = np.zeros((n_u,n_s))#np.random.randn(n_u,n_s)
u_0_cas = MX.sym("u_0", (n_u, 1))
k_fb_cas_0 = MX.sym("k_fb", (T - 1, n_u * n_s))
k_ff_cas = MX.sym("k_ff", (T - 1, n_u))
p_new_cas, q_new_cas, _ = reach_cas.multi_step_reachability(
p, u_0, k_fb_cas_0, k_ff_cas, gp, L_mu, L_sigm, c_safety, a, b)
f = Function("f", [u_0_cas, k_fb_cas_0, k_ff_cas], [p_new_cas, q_new_cas])
k_fb_0_cas = np.copy(k_fb_0) # np.copy(k_fb_0)
for i in range(T - 1):
k_fb_0_cas[i, None, :] = k_fb_0_cas[i, None, :] + cas_reshape(
k_fb, (1, n_u * n_s))
p_all_cas, q_all_cas = f(u_0, k_fb_0_cas, k_ff)
k_ff_all = np.vstack((u_0.T, k_ff))
k_fb_apply = array_of_vec_to_array_of_mat(k_fb_0, n_u, n_s)
for i in range(T - 1):
k_fb_apply[i, :, :] += k_fb
_, _, p_all_num, q_all_num = reach_num.multistep_reachability(
p, gp, k_fb_apply, k_ff_all, L_mu, L_sigm, None, c_safety, 0, a, b,
None)
assert np.allclose(p_all_cas, p_all_num, r_tol,
a_tol), "Are the centers of the ellipsoids same?"
assert np.allclose(q_all_cas[0, :], q_all_num[0, :, :].reshape(
(-1, n_s * n_s)), r_tol,
a_tol), "Are the first shape matrices the same?"
assert np.allclose(q_all_cas[1, :], q_all_num[1, :, :].reshape(
(-1, n_s * n_s)), r_tol,
a_tol), "Are the second shape matrices the same?"
# assert np.allclose(q_all_cas[1,:],q_all_num[1,:].reshape((-1,n_s*n_s))), "Are the second shape matrices the same?"
assert np.allclose(q_all_cas[-1, :], q_all_num[-1, :, :].reshape(
(-1, n_s * n_s)), r_tol,
a_tol), "Are the last shape matrices the same?"
assert np.allclose(q_all_cas, q_all_num.reshape((T, n_s * n_s)), r_tol,
a_tol), "Are the shape matrices the same?"
def shift_knot1_fwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = MX.sym('cfs', cfs.shape)
t_shift_sym = MX.sym('t_shift')
T = shiftfirstknot_T(basis, t_shift_sym)
cfs2_sym = mtimes(T, cfs_sym)
fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand()
return fun(cfs, t_shift)
else:
T = shiftfirstknot_T(basis, t_shift)
return T.dot(cfs)
def shift_knot1_fwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = MX.sym('cfs', cfs.shape)
t_shift_sym = MX.sym('t_shift')
T = shiftfirstknot_T(basis, t_shift_sym)
cfs2_sym = mtimes(T, cfs_sym)
fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand()
return fun(cfs, t_shift)
else:
T = shiftfirstknot_T(basis, t_shift)
return T.dot(cfs)
def test_loss_sat(before_loss_sat):
""" Does saturating cost function return same as Matlab implementation?"""
m, v, W, z, L = before_loss_sat
m_cas = MX.sym('m', (3, 1))
v_cas = MX.sym('v_cas', (3, 3))
L_sym = utils_cas.loss_sat(m_cas, v_cas, z, W)
f = Function('loss_sat', [m_cas, v_cas], [L_sym])
L_out = f(m, v)
assert np.allclose(L, L_out, atol=1e-3), "Are the losses the same?"
def construct_upd_z(self, problem=None, lineq_updz=True):
if problem is not None:
self.problem_upd_z = problem
self._lineq_updz = lineq_updz
return 0.
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
raise ValueError('For now, only equality constrained QP ' +
'z-updates are allowed!')
x_i = struct_symMX(self.q_i_struct)
x_j = struct_symMX(self.q_ij_struct)
l_i = struct_symMX(self.q_i_struct)
l_ij = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
par = struct_symMX(self.par_global_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t / T
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A(par.cat)
b = b(par.cat)
# build KKT system and solve it via schur complement method
l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
f = -(l + rho * x)
G = -(1 / rho) * mtimes(A, A.T)
h = b + (1 / rho) * mtimes(A, f)
mu = solve(G, h)
z = -(1 / rho) * (mtimes(A.T, mu) + f)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi + l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, buildtime = create_function('upd_z_' + str(self._index), inp,
out, self.options)
self.problem_upd_z = prob
return buildtime
def force_func(biorbd_model, use_activation=False):
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)
return Function(
"MuscleForce",
[qMX, dqMX, aMX, uMX],
[muscles_forces(qMX, dqMX, aMX, uMX, biorbd_model, use_activation=use_activation)],
["qMX", "dqMX", "aMX", "uMX"],
["Force"],
).expand()
def construct_upd_z(self, problem=None, lineq_updz=True):
if problem is not None:
self.problem_upd_z = problem
self._lineq_updz = lineq_updz
return 0.
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
raise ValueError('For now, only equality constrained QP ' +
'z-updates are allowed!')
x_i = struct_symMX(self.q_i_struct)
x_j = struct_symMX(self.q_ij_struct)
l_i = struct_symMX(self.q_i_struct)
l_ij = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
par = struct_symMX(self.par_global_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t/T
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A(par.cat)
b = b(par.cat)
# build KKT system and solve it via schur complement method
l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
f = -(l + rho*x)
G = -(1/rho)*mtimes(A, A.T)
h = b + (1/rho)*mtimes(A, f)
mu = solve(G, h)
z = -(1/rho)*(mtimes(A.T, mu)+f)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options)
self.problem_upd_z = prob
return buildtime
def intg_builtin(self, f, X, U, P, Z):
# A single CVODES step
DT = MX.sym("DT")
t = MX.sym("t")
t0 = MX.sym("t0")
res = f(x=X, u=U, p=P, t=t0+t*DT, z=Z)
data = {'x': X, 'p': vertcat(U, DT, P, t0), 'z': Z, 't': t, 'ode': DT * res["ode"], 'quad': DT * res["quad"], 'alg': res["alg"]}
options = dict(self.intg_options)
if self.intg in ["collocation"]:
options["number_of_finite_elements"] = 1
I = integrator('intg_cvodes', self.intg, data, options)
res = I.call({'x0': X, 'p': vertcat(U, DT, P, t0)})
return Function('F', [X, U, t0, DT, P], [res["xf"], MX(), res["qf"]], ['x0', 'u', 't0', 'DT', 'p'], ['xf', 'poly_coeff','qf'])
def __configure_forward_dyn_func(nlp, dyn_func):
nlp["nu"] = nlp["u"].rows()
nlp["nx"] = nlp["x"].rows()
symbolic_states = MX.sym("x", nlp["nx"], 1)
symbolic_controls = MX.sym("u", nlp["nu"], 1)
nlp["dynamics_func"] = Function(
"ForwardDyn",
[symbolic_states, symbolic_controls],
[dyn_func(symbolic_states, symbolic_controls, nlp)],
["x", "u"],
["xdot"],
).expand() # .map(nlp["ns"], "thread", 2)
def configure_contact(ocp, nlp, dyn_func: Callable):
"""
Configure the contact points
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
nlp: NonLinearProgram
A reference to the phase
dyn_func: Callable[states, controls, param]
The function to get the values of contact forces from the dynamics
"""
symbolic_states = MX.sym("x", nlp.nx, 1)
symbolic_controls = MX.sym("u", nlp.nu, 1)
symbolic_param = MX.sym("p", nlp.np, 1)
nlp.contact_forces_func = Function(
"contact_forces_func",
[symbolic_states, symbolic_controls, symbolic_param],
[
dyn_func(symbolic_states, symbolic_controls, symbolic_param,
nlp)
],
["x", "u", "p"],
["contact_forces"],
).expand()
all_contact_names = []
for elt in ocp.nlp:
all_contact_names.extend([
name.to_string() for name in elt.model.contactNames()
if name.to_string() not in all_contact_names
])
if "contact_forces" in nlp.mapping["plot"]:
phase_mappings = nlp.mapping["plot"]["contact_forces"]
else:
contact_names_in_phase = [
name.to_string() for name in nlp.model.contactNames()
]
phase_mappings = Mapping([
i for i, c in enumerate(all_contact_names)
if c in contact_names_in_phase
])
nlp.plot["contact_forces"] = CustomPlot(nlp.contact_forces_func,
axes_idx=phase_mappings,
legend=all_contact_names)
def test_forward_dynamics_with_external_forces(brbd):
m = brbd.Model("../../models/pyomecaman_withActuators.bioMod")
q = np.array([i * 1.1 for i in range(m.nbQ())])
qdot = np.array([i * 1.1 for i in range(m.nbQ())])
tau = np.array([i * 1.1 for i in range(m.nbQ())])
# With external forces
sv1 = np.array(
((11.1, 22.2, 33.3, 44.4, 55.5, 66.6), (11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2))
).T
f_ext = brbd.to_spatial_vector(sv1)
if brbd.currentLinearAlgebraBackend() == 1:
from casadi import MX
q_sym = MX.sym("q", m.nbQ(), 1)
qdot_sym = MX.sym("qdot", m.nbQdot(), 1)
tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1)
forward_dynamics = brbd.to_casadi_func("ForwardDynamics", m.ForwardDynamics, q_sym, qdot_sym, tau_sym, f_ext)
qddot = forward_dynamics(q, qdot, tau)
qddot = np.array(qddot)[:, 0]
elif brbd.currentLinearAlgebraBackend() == 0:
# if Eigen backend is used
qddot = m.ForwardDynamics(q, qdot, tau, f_ext).to_array()
else:
raise NotImplementedError("Backend not implemented in test")
qddot_expected = np.array(
[
8.8871711208009998,
-13.647827029817943,
-33.606145294752132,
16.922669487341341,
-21.882821189868423,
41.15364990805439,
68.892537246574463,
-324.59756885799197,
-447.99217990207387,
18884.241415786601,
-331.24622725851572,
1364.7620674666462,
3948.4748602722384,
]
)
np.testing.assert_almost_equal(qddot, qddot_expected)
def __configure_tau(nlp, as_states, as_controls):
"""
Configures common settings for torque driven problems with and without contacts.
:param nlp: An OptimalControlProgram class.
"""
if nlp["tau_mapping"] is None:
nlp["tau_mapping"] = BidirectionalMapping(
Mapping(range(nlp["model"].nbGeneralizedTorque())),
Mapping(range(nlp["model"].nbGeneralizedTorque())))
dof_names = nlp["model"].nameDof()
u = MX()
for i in nlp["tau_mapping"].reduce.map_idx:
u = vertcat(u, MX.sym("Tau_" + dof_names[i].to_string(), 1, 1))
nlp["nbTau"] = nlp["tau_mapping"].reduce.len
legend_tau = [
"tau_" + nlp["model"].nameDof()[idx].to_string()
for idx in nlp["tau_mapping"].reduce.map_idx
]
if as_states:
nlp["x"] = u
nlp["var_states"] = {"tau": nlp["nbTau"]}
# Add plot if it happens
if as_controls:
nlp["u"] = u
nlp["var_controls"] = {"tau": nlp["nbTau"]}
nlp["plot"]["tau"] = CustomPlot(lambda x, u: u[:nlp["nbTau"]],
plot_type=PlotType.STEP,
legend=legend_tau)
def new_variable(self, name, dim, init, lb, ub):
"""
Add a new variable to the problem.
Parameters
----------
name : string
Variable name.
dim : int
Number of dimensions.
init : array, shape=(dim,)
Initial values.
lb : array, shape=(dim,)
Vector of lower bounds on variable values.
ub : array, shape=(dim,)
Vector of upper bounds on variable values.
"""
assert len(init) == len(lb) == len(ub) == dim
var = MX.sym(name, dim)
self.var_symbols.append(var)
self.initvals += init
self.var_index[name] = len(self.var_lbounds)
self.var_lbounds += lb
self.var_ubounds += ub
return var
def _define_mx(self, name, size0, size1, dictionary, value=None):
if value is None:
value = np.zeros((size0, size1))
symbol_name = self._add_label(name)
dictionary[name] = MX.sym(symbol_name, size0, size1)
self._values[name] = value
self.add_to_dict(dictionary[name], name)
return dictionary[name]
def construct_upd_l(self, problem=None):
if problem is not None:
self.problem_upd_l = problem
return 0.
# create parameters
z_ij = struct_symMX(self.q_ij_struct)
l_ij = struct_symMX(self.q_ij_struct)
x_j = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
inp = [x_j, z_ij, l_ij, t, T, rho]
# update lambda
l_ij_new = self.q_ij_struct(l_ij.cat + rho*(x_j.cat - z_ij.cat))
out = [l_ij_new]
# create problem
prob, buildtime = create_function('upd_l_'+str(self._index), inp, out, self.options)
self.problem_upd_l = prob
return buildtime
def construct_upd_res(self, problem=None):
if problem is not None:
self.problem_upd_res = problem
return 0.
# create parameters
x_i = struct_symMX(self.q_i_struct)
z_i = struct_symMX(self.q_i_struct)
z_i_p = struct_symMX(self.q_i_struct)
z_ij = struct_symMX(self.q_ij_struct)
z_ij_p = struct_symMX(self.q_ij_struct)
x_j = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
t0 = t/T
rho = MX.sym('rho')
inp = [x_i, z_i, z_i_p, z_ij, z_ij_p, x_j, t, T, rho]
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
z_i = self.q_i_struct(z_i)
z_i_p = self.q_i_struct(z_i_p)
z_ij = self.q_ij_struct(z_ij)
z_ij_p = self.q_ij_struct(z_ij_p)
x_j = self.q_ij_struct(x_j)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, z_i_p], tf, self.q_i)
self._transform_spline([x_j, z_ij, z_ij_p], tf, self.q_ij)
# compute residuals
pr = mtimes((x_i.cat-z_i.cat).T, (x_i.cat-z_i.cat))
pr += mtimes((x_j.cat-z_ij.cat).T, (x_j.cat-z_ij.cat))
dr = rho*mtimes((z_i.cat-z_i_p.cat).T, (z_i.cat-z_i_p.cat))
dr += rho*mtimes((z_ij.cat-z_ij_p.cat).T, (z_ij.cat-z_ij_p.cat))
cr = rho*pr + dr
out = [pr, dr, cr]
# create problem
prob, buildtime = create_function('upd_res_'+str(self._index), inp, out, self.options)
self.problem_upd_res = prob
return buildtime
