def test_mhe_redim_xbounds_and_init():
root_folder = TestUtils.bioptim_folder() + "/examples/moving_horizon_estimation/"
biorbd_model = biorbd.Model(root_folder + "cart_pendulum.bioMod")
nq = biorbd_model.nbQ()
ntau = biorbd_model.nbGeneralizedTorque()
n_cycles = 3
window_len = 5
window_duration = 0.2
x_init = InitialGuess(np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT)
u_init = InitialGuess(np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT)
x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT)
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)))
mhe = MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
n_threads=4,
)
def update_functions(mhe, t, _):
return t < n_cycles
mhe.solve(update_functions, Solver.IPOPT)
def test_acados_bounds_not_implemented(failing):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
root_folder = TestUtils.bioptim_folder(
) + "/examples/moving_horizon_estimation/"
biorbd_model = biorbd.Model(root_folder + "models/cart_pendulum.bioMod")
nq = biorbd_model.nbQ()
ntau = biorbd_model.nbGeneralizedTorque()
n_cycles = 3
window_len = 5
window_duration = 0.2
x_init = InitialGuess(np.zeros((nq * 2, 1)),
interpolation=InterpolationType.CONSTANT)
u_init = InitialGuess(np.zeros((ntau, 1)),
interpolation=InterpolationType.CONSTANT)
if failing == "u_bounds":
x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)))
u_bounds = Bounds(np.zeros((ntau, 1)),
np.zeros((ntau, 1)),
interpolation=InterpolationType.CONSTANT)
elif failing == "x_bounds":
x_bounds = Bounds(np.zeros((nq * 2, 1)),
np.zeros((nq * 2, 1)),
interpolation=InterpolationType.CONSTANT)
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)))
else:
raise ValueError("Wrong value for failing")
mhe = MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
n_threads=4,
)
def update_functions(mhe, t, _):
return t < n_cycles
with pytest.raises(
NotImplementedError,
match=
f"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT for the {failing}",
):
mhe.solve(update_functions, Solver.ACADOS)
def _set_generic_ocp(self):
self.ocp = MovingHorizonEstimator(
biorbd_model=self.model,
dynamics=self.dynamics,
window_len=self.n_shooting,
window_duration=self.time,
x_init=self.x_init,
u_init=self.u_init,
x_bounds=self.x_bounds,
u_bounds=self.u_bounds,
objective_functions=self.objective_functions,
constraints=self.constraints,
use_sx=self.solver == Solver.ACADOS,
n_threads=self.n_threads,
)
def test_mhe_redim_xbounds_not_implemented():
root_folder = TestUtils.bioptim_folder(
) + "/examples/moving_horizon_estimation/"
biorbd_model = biorbd.Model(root_folder + "models/cart_pendulum.bioMod")
nq = biorbd_model.nbQ()
ntau = biorbd_model.nbGeneralizedTorque()
n_cycles = 3
window_len = 5
window_duration = 0.2
x_init = InitialGuess(np.zeros((nq * 2, 1)),
interpolation=InterpolationType.CONSTANT)
u_init = InitialGuess(np.zeros((ntau, 1)),
interpolation=InterpolationType.CONSTANT)
x_bounds = Bounds(
np.zeros((nq * 2, window_len + 1)),
np.zeros((nq * 2, window_len + 1)),
interpolation=InterpolationType.EACH_FRAME,
)
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)))
mhe = MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
n_threads=4,
)
def update_functions(mhe, t, _):
return t < n_cycles
with pytest.raises(
NotImplementedError,
match="The MHE is not implemented yet for x_bounds not being "
"CONSTANT or CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT",
):
mhe.solve(update_functions, Solver.IPOPT)
class ViolinNMPC(ViolinOcp):
def __init__(
self,
model_path: str,
violin: Violin,
bow: Bow,
bow_starting: BowPosition.TIP,
use_muscles: bool = False,
window_duration: float = 1,
window_len: int = 30,
solver: Solver = Solver.ACADOS,
n_threads: int = 8,
):
super(ViolinNMPC, self).__init__(
model_path=model_path,
violin=violin,
bow=bow,
n_cycles=1,
bow_starting=bow_starting,
use_muscles=use_muscles,
time_per_cycle=window_duration,
n_shooting_per_cycle=window_len,
solver=solver,
n_threads=n_threads,
)
def _set_generic_ocp(self):
self.ocp = MovingHorizonEstimator(
biorbd_model=self.model,
dynamics=self.dynamics,
window_len=self.n_shooting,
window_duration=self.time,
x_init=self.x_init,
u_init=self.u_init,
x_bounds=self.x_bounds,
u_bounds=self.u_bounds,
objective_functions=self.objective_functions,
constraints=self.constraints,
use_sx=self.solver == Solver.ACADOS,
n_threads=self.n_threads,
)
def solve(self, update_function, **opts: Any) -> Solution:
return self.ocp.solve(update_function, solver=self.solver, **opts)
def prepare_mhe(biorbd_model, window_len, window_duration, max_torque, x_init,
u_init):
new_objectives = Objective(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS,
node=Node.ALL,
weight=1000,
list_index=0)
return MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
objective_functions=new_objectives,
x_init=InitialGuess(x_init,
interpolation=InterpolationType.EACH_FRAME),
u_init=InitialGuess(u_init,
interpolation=InterpolationType.EACH_FRAME),
x_bounds=QAndQDotBounds(biorbd_model),
u_bounds=Bounds([-max_torque, 0.0], [max_torque, 0.0]),
n_threads=4,
)
def load(file_path: str):
return MovingHorizonEstimator.load(file_path)