This example shows how to define dynamics with an implicit differential equation.
#include <OpenSim/Moco/osimMoco.h>
class MyImplicitAuxiliaryDynamics :
public Component {
OpenSim_DECLARE_CONCRETE_OBJECT(MyImplicitAuxiliaryDynamics,
Component);
public:
OpenSim_DECLARE_PROPERTY(
default_foo, double, "Default value of the state variable foo.");
OpenSim_DECLARE_PROPERTY(foo_dynamics_mode, std::string,
"The dynamics mode for enforcing y y' - 1 = 0. "
"Options: 'explicit' or 'implicit'. Default: 'explicit'. ");
OpenSim_DECLARE_OUTPUT(
statebounds_foo, SimTK::Vec2, getBoundsFoo, SimTK::Stage::Model);
OpenSim_DECLARE_OUTPUT(implicitresidual_foo, double, getImplicitResidualFoo,
SimTK::Stage::Dynamics);
OpenSim_DECLARE_OUTPUT(implicitenabled_foo, bool, getImplicitEnabledFoo,
SimTK::Stage::Model);
MyImplicitAuxiliaryDynamics() {
setName("implicit_auxdyn");
constructProperty_default_foo(0.5);
constructProperty_foo_dynamics_mode("explicit");
}
MyImplicitAuxiliaryDynamics(std::string foo_dynamics_mode)
: MyImplicitAuxiliaryDynamics() {
set_foo_dynamics_mode(foo_dynamics_mode);
}
SimTK::Vec2 getBoundsFoo(const SimTK::State&) const {
return SimTK::Vec2(0, 3);
}
bool getImplicitEnabledFoo(const SimTK::State&) const { return true; }
double getImplicitResidualFoo(const SimTK::State& s) const {
if (!m_foo_dynamics_mode_implicit) { return SimTK::NaN; }
if (!isCacheVariableValid(s, "implicitresidual_foo")) {
const double derivative =
getDiscreteVariableValue(s, "implicitderiv_foo");
const double statevar = getStateVariableValue(s, "foo");
double residual = derivative * statevar - 1;
setCacheVariableValue(s, "implicitresidual_foo", residual);
markCacheVariableValid(s, "implicitresidual_foo");
}
return getCacheVariableValue<double>(s, "implicitresidual_foo");
}
private:
void extendFinalizeFromProperties() override {
OPENSIM_THROW_IF_FRMOBJ(get_foo_dynamics_mode() != "implicit" &&
get_foo_dynamics_mode() != "explicit",
"Expected foo_dynamics_mode to be 'implicit' or "
"'explicit', but got '{}'.",
get_foo_dynamics_mode());
m_foo_dynamics_mode_implicit = get_foo_dynamics_mode() == "implicit";
}
void extendInitStateFromProperties(SimTK::State& s) const override {
Super::extendInitStateFromProperties(s);
setStateVariableValue(s, "foo", get_default_foo());
}
void extendSetPropertiesFromState(const SimTK::State& s) override {
Super::extendSetPropertiesFromState(s);
set_default_foo(getStateVariableValue(s, "foo"));
}
void computeStateVariableDerivatives(const SimTK::State& s) const override {
if (m_foo_dynamics_mode_implicit) {
const double derivative =
getDiscreteVariableValue(s, "implicitderiv_foo");
setStateVariableDerivativeValue(s, "foo", derivative);
} else {
const double statevar = getStateVariableValue(s, "foo");
setStateVariableDerivativeValue(s, "foo", 1.0 / statevar);
}
}
void extendAddToSystem(SimTK::MultibodySystem& system) const override {
Super::extendAddToSystem(system);
addStateVariable("foo");
addDiscreteVariable("implicitderiv_foo", SimTK::Stage::Dynamics);
addCacheVariable(
"implicitresidual_foo", double(0), SimTK::Stage::Dynamics);
}
bool m_foo_dynamics_mode_implicit;
};
int main() {
try {
double finalFooTimeStepping;
double finalFooDircol;
{
model.
addComponent(
new MyImplicitAuxiliaryDynamics(
"explicit"));
state = manager.integrate(1.0);
finalFooTimeStepping =
}
{
auto model = OpenSim::make_unique<Model>();
model->
addComponent(
new MyImplicitAuxiliaryDynamics(
"implicit"));
problem.setModel(std::move(model));
problem.setTimeBounds(0, 1.0);
problem.setStateInfo("/implicit_auxdyn/foo", {0, 3}, 1.0);
}
std::cout << "Final foo with time-stepping: " << finalFooTimeStepping
<< "\nFinal foo with Moco: " << finalFooDircol << std::endl;
} catch (const std::exception& e) {
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
} catch (...) { return EXIT_FAILURE; }
return EXIT_SUCCESS;
}