28 const std::optional<ConstVectorRef> &gap = std::nullopt,
29 const uint max_iters = 1000,
const T EPS = 1e-6) {
30 using ExpModel = ExplicitDynamicsModelTpl<T>;
31 using ExpData = ExplicitDynamicsDataTpl<T>;
33 if (model.is_explicit()) {
34 const ExpModel &model_cast =
static_cast<const ExpModel &
>(model);
35 ExpData &data_cast =
static_cast<ExpData &
>(data);
36 run(model_cast, x, u, data_cast, xout, gap);
44 model.evaluate(x, u, xnext, data);
50 model.computeJacobians(x, u, xnext, data);
53 x, xout, data.value_, dx0buf, data.Jy_, EPS, max_iters);
57 static void run(
const ExplicitDynamicsModelTpl<T> &model,
59 ExplicitDynamicsDataTpl<T> &data,
VectorRef xout,
60 const std::optional<ConstVectorRef> &gap = std::nullopt) {
61 model.forward(x, u, data);
63 if (gap.has_value()) {
64 model.space_next().integrate(xout, *gap, xout);