API  4.5
For C++ developers
exampleMarkerTracking.cpp

This is a simple example of using a marker tracking goal.

/* -------------------------------------------------------------------------- *
* OpenSim Moco: exampleMarkerTracking.cpp *
* -------------------------------------------------------------------------- *
* Copyright (c) 2017 Stanford University and the Authors *
* *
* Author(s): Nicholas Bianco *
* *
* Licensed under the Apache License, Version 2.0 (the "License"); you may *
* not use this file except in compliance with the License. You may obtain a *
* copy of the License at http://www.apache.org/licenses/LICENSE-2.0 *
* *
* Unless required by applicable law or agreed to in writing, software *
* distributed under the License is distributed on an "AS IS" BASIS, *
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
* See the License for the specific language governing permissions and *
* limitations under the License. *
* -------------------------------------------------------------------------- */
#include <OpenSim/Simulation/SimbodyEngine/PinJoint.h>
#include <OpenSim/Simulation/MarkersReference.h>
#include <OpenSim/Actuators/CoordinateActuator.h>
#include <OpenSim/Moco/osimMoco.h>
using namespace OpenSim;
std::unique_ptr<Model> createDoublePendulumModel() {
// This function is similar to ModelFactory::createNLinkPendulum().
auto model = make_unique<Model>();
model->setName("double_pendulum");
using SimTK::Vec3;
using SimTK::Inertia;
// Create two links, each with a mass of 1 kg, center of mass at the body's
// origin, and moments and products of inertia of zero.
auto* b0 = new OpenSim::Body("b0", 1, Vec3(0), Inertia(1));
model->addBody(b0);
auto* b1 = new OpenSim::Body("b1", 1, Vec3(0), Inertia(1));
model->addBody(b1);
// Add markers to body origin locations
auto* m0 = new Marker("m0", *b0, Vec3(0));
auto* m1 = new Marker("m1", *b1, Vec3(0));
model->addMarker(m0);
model->addMarker(m1);
// Connect the bodies with pin joints. Assume each body is 1 m long.
auto* j0 = new PinJoint("j0", model->getGround(), Vec3(0), Vec3(0),
*b0, Vec3(-1, 0, 0), Vec3(0));
auto& q0 = j0->updCoordinate();
q0.setRangeMin(-10);
q0.setRangeMax(10);
q0.setName("q0");
auto* j1 = new PinJoint("j1",
*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);
auto* tau0 = new CoordinateActuator();
tau0->setCoordinate(&j0->updCoordinate());
tau0->setName("tau0");
tau0->setOptimalForce(1);
tau0->setMinControl(-40);
tau0->setMaxControl(40);
model->addForce(tau0);
auto* tau1 = new CoordinateActuator();
tau1->setCoordinate(&j1->updCoordinate());
tau1->setName("tau1");
tau1->setOptimalForce(1);
tau1->setMinControl(-40);
tau1->setMaxControl(40);
model->addForce(tau1);
// Add display geometry.
SimTK::Transform transform(SimTK::Vec3(-0.5, 0, 0));
auto* b0Center = new PhysicalOffsetFrame("b0_center", *b0, transform);
b0->addComponent(b0Center);
b0Center->attachGeometry(new Ellipsoid(0.5, 0.1, 0.1));
auto* b1Center = new PhysicalOffsetFrame("b1_center", *b1, transform);
b1->addComponent(b1Center);
b1Center->attachGeometry(new Ellipsoid(0.5, 0.1, 0.1));
model->finalizeConnections();
return model;
}
int main() {
MocoStudy study;
study.setName("double_pendulum_marker_tracking");
// Define the optimal control problem.
// ===================================
MocoProblem& problem = study.updProblem();
// Model (dynamics).
// -----------------
problem.setModel(createDoublePendulumModel());
// Bounds.
// -------
double finalTime = 1.0;
problem.setTimeBounds(0, finalTime);
// Cost.
// -----
// Create marker trajectories based on exampleTracking.cpp joint angles.
TimeSeriesTableVec3 markerTrajectories;
markerTrajectories.setColumnLabels({"/markerset/m0", "/markerset/m1"});
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});
}
// Assign a weight to each marker.
Set<MarkerWeight> markerWeights;
markerWeights.cloneAndAppend({"/markerset/m0", 100});
markerWeights.cloneAndAppend({"/markerset/m1", 10});
// Create the MarkersReference to be passed to the cost.
MarkersReference ref(markerTrajectories, markerWeights);
// Create cost, set reference, and attach to problem.
auto* markerTracking = problem.addGoal<MocoMarkerTrackingGoal>();
markerTracking->setMarkersReference(ref);
// Configure the solver.
// =====================
auto& solver = study.initCasADiSolver();
solver.set_verbosity(2);
// Use a full Newton optimization algorithm (compute the entire Hessian)
// rather than a quasi-Newton algorithm (approximating the Hessian from
// successive gradients).
solver.set_optim_hessian_approximation("exact");
// Solve the problem.
// ==================
MocoSolution solution = study.solve();
solution.write("exampleMarkerTracking_solution.sto");
study.visualize(solution);
return EXIT_SUCCESS;
}