This example shows setting solver settings, customizing an initial guess, and defining a custom goal (cost term).See Defining a custom goal or cost.
#include <Moco/osimMoco.h>
#include <OpenSim/Actuators/CoordinateActuator.h>
#include <OpenSim/Simulation/SimbodyEngine/SliderJoint.h>
std::unique_ptr<Model> createSlidingMassModel() {
auto model = make_unique<Model>();
model->setName("sliding_mass");
model->set_gravity(SimTK::Vec3(0, 0, 0));
auto* body = new Body("body", 2.0, SimTK::Vec3(0), SimTK::Inertia(0));
model->addComponent(body);
auto* joint = new SliderJoint("slider", model->getGround(), *body);
auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX);
coord.setName("position");
model->addComponent(joint);
auto* actu = new CoordinateActuator();
actu->setCoordinate(&coord);
actu->setName("actuator");
actu->setOptimalForce(1);
model->addComponent(actu);
body->attachGeometry(new Sphere(0.05));
model->finalizeConnections();
return model;
}
class MocoCustomEffortGoal :
public MocoGoal {
OpenSim_DECLARE_CONCRETE_OBJECT(MocoCustomEffortGoal,
MocoGoal);
public:
MocoCustomEffortGoal() {}
MocoCustomEffortGoal(std::string name) :
MocoGoal(std::move(name)) {}
MocoCustomEffortGoal(std::string name, double weight)
protected:
Mode getDefaultModeImpl() const override { return Mode::Cost; }
bool getSupportsEndpointConstraintImpl() const override { return true; }
void initializeOnModelImpl(const Model&) const override {
setNumIntegralsAndOutputs(1, 1);
}
void calcIntegrandImpl(
const SimTK::State& state, double& integrand) const override {
getModel().realizeVelocity(state);
const auto&
controls = getModel().getControls(state);
}
void calcGoalImpl(
const GoalInput& input, SimTK::Vector& cost) const override {
cost[0] = input.integral;
}
};
int main() {
study.setName("sliding_mass");
problem.
setModel(createSlidingMassModel());
problem.
setStateInfo(
"/slider/position/speed", {-50, 50}, 0, 0);
problem.
addGoal<MocoCustomEffortGoal>(
"effort");
solver.set_num_mesh_intervals(50);
solver.set_verbosity(2);
solver.set_optim_solver("ipopt");
solver.set_optim_ipopt_print_level(4);
solver.set_optim_max_iterations(50);
solver.set_optim_finite_difference_scheme("forward");
solver.set_optim_sparsity_detection("random");
solver.set_optim_write_sparsity("sliding_mass");
guess.
setState(
"/slider/position/value", {0.0, 1.0});
solver.setGuess(guess);
std::cout <<
"Solution status: " << solution.
getStatus() << std::endl;
return EXIT_SUCCESS;
}