This is a simple example of using a marker tracking goal.
#include <OpenSim/Simulation/SimbodyEngine/PinJoint.h>
#include <OpenSim/Simulation/MarkersReference.h>
#include <OpenSim/Actuators/CoordinateActuator.h>
#include <OpenSim/Moco/osimMoco.h>
std::unique_ptr<Model> createDoublePendulumModel() {
auto model = make_unique<Model>();
model->setName("double_pendulum");
using SimTK::Vec3;
using SimTK::Inertia;
model->addBody(b0);
model->addBody(b1);
auto* m0 =
new Marker(
"m0", *b0, Vec3(0));
auto* m1 =
new Marker(
"m1", *b1, Vec3(0));
model->addMarker(m0);
model->addMarker(m1);
auto* j0 =
new PinJoint(
"j0", model->getGround(), Vec3(0), Vec3(0),
*b0, Vec3(-1, 0, 0), Vec3(0));
q0.setRangeMin(-10);
q0.setRangeMax(10);
*b0, Vec3(0), Vec3(0), *b1, Vec3(-1, 0, 0), Vec3(0));
auto& q1 = j1->updCoordinate();
q1.setRangeMin(-10);
q1.setRangeMax(10);
q1.setName("q1");
model->addJoint(j0);
model->addJoint(j1);
tau0->setCoordinate(&j0->updCoordinate());
tau0->setName("tau0");
tau0->setOptimalForce(1);
tau0->setMinControl(-40);
tau0->setMaxControl(40);
model->addForce(tau0);
tau1->setCoordinate(&j1->updCoordinate());
tau1->setName("tau1");
tau1->setOptimalForce(1);
tau1->setMinControl(-40);
tau1->setMaxControl(40);
model->addForce(tau1);
SimTK::Transform transform(SimTK::Vec3(-0.5, 0, 0));
b0->addComponent(b0Center);
b0Center->attachGeometry(
new Ellipsoid(0.5, 0.1, 0.1));
b1->addComponent(b1Center);
b1Center->attachGeometry(
new Ellipsoid(0.5, 0.1, 0.1));
model->finalizeConnections();
return model;
}
int main() {
study.
setName(
"double_pendulum_marker_tracking");
problem.
setModel(createDoublePendulumModel());
double finalTime = 1.0;
for (double time = 0; time < finalTime; time += 0.01) {
SimTK::Real q0 = (time / 1.0) * 0.5 * SimTK::Pi;
SimTK::Real q1 = (time / 1.0) * 0.25 * SimTK::Pi;
SimTK::Vec3 m0(cos(q0), sin(q0), 0);
SimTK::Vec3 m1 = m0 + SimTK::Vec3(cos(q0 + q1), sin(q0 + q1), 0);
markerTrajectories.
appendRow(time, {m0, m1});
}
solver.set_verbosity(2);
solver.set_optim_hessian_approximation("exact");
solution.
write(
"exampleMarkerTracking_solution.sto");
return EXIT_SUCCESS;
}