int main() {
Model model("gait10dof18musc_subject01.osim");
// Create a StatesTrajectory from a forward simulation.
// ----------------------------------------------------
// To assist with creating interesting (non-zero) coordinate values:
model.updCoordinateSet().get("pelvis_ty").setDefaultLocked(true);
auto& initState = model.initSystem();
StatesTrajectory states = simulate(model, initState, 0.05);
// Use trajectory.
// ---------------
for (const auto& state : states) {
std::cout << state.getTime() << "s: "
<< model.getStateVariableValue(state,
"ankle_r/ankle_angle_r/value")
<< std::endl;
}
// Export to data table.
// ---------------------
auto table = states.exportToTable(model);
return 0;
}
void testCopying() {
Model model("gait2354_simbody.osim");
auto& state = model.initSystem();
StatesTrajectory states;
{
state.setTime(0.5);
states.append(state);
state.setTime(1.3);
states.append(state);
state.setTime(3.5);
states.append(state);
}
{
StatesTrajectory statesCopyConstruct(states);
// Ideally we'd check for equality (operator==()), but State does not
// have an equality operator.
SimTK_TEST_EQ((int)statesCopyConstruct.getSize(), (int)states.getSize());
for (size_t i = 0; i < states.getSize(); ++i) {
SimTK_TEST_EQ(statesCopyConstruct[i].getTime(), states[i].getTime());
}
}
{
StatesTrajectory statesCopyAssign;
statesCopyAssign = states;
SimTK_TEST_EQ((int)statesCopyAssign.getSize(), (int)states.getSize());
for (size_t i = 0; i < states.getSize(); ++i) {
SimTK_TEST_EQ(statesCopyAssign[i].getTime(), states[i].getTime());
}
}
}
void testFrontBack() {
Model model("arm26.osim");
const auto& state = model.initSystem();
StatesTrajectory states;
states.append(state);
states.append(state);
states.append(state);
SimTK_TEST(&states.front() == &states[0]);
SimTK_TEST(&states.back() == &states[2]);
}
void testAppendTimesAreNonDecreasing() {
Model model("gait2354_simbody.osim");
auto& state = model.initSystem();
state.setTime(1.0);
StatesTrajectory states;
states.append(state);
// Multiple states can have the same time; does not throw an exception:
states.append(state);
state.setTime(0.9999);
SimTK_TEST_MUST_THROW_EXC(states.append(state),
SimTK::Exception::APIArgcheckFailed);
}
StatesTrajectory simulate(const Model& model, const State& initState,
double finalTime) {
StatesTrajectory states;
SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem());
SimTK::TimeStepper ts(model.getSystem(), integrator);
ts.initialize(initState);
ts.setReportAllSignificantStates(true);
integrator.setReturnEveryInternalStep(true);
while (ts.getState().getTime() < finalTime) {
ts.stepTo(finalTime);
// StatesTrajectory API for appending states:
states.append(ts.getState());
}
return states;
}
void tableAndTrajectoryMatch(const Model& model,
const TimeSeriesTable& table,
const StatesTrajectory& states,
std::vector<std::string> columns = {}) {
const auto stateNames = model.getStateVariableNames();
size_t numColumns{};
if (columns.empty()) {
numColumns = stateNames.getSize();
} else {
numColumns = columns.size();
}
SimTK_TEST(table.getNumColumns() == numColumns);
SimTK_TEST(table.getNumRows() == states.getSize());
const auto& colNames = table.getColumnLabels();
// Test that the data table has exactly the same numbers.
for (size_t itime = 0; itime < states.getSize(); ++itime) {
// Test time.
SimTK_TEST(table.getIndependentColumn()[itime] ==
states[itime].getTime());
// Test state values.
for (size_t icol = 0; icol < table.getNumColumns(); ++icol) {
const auto& stateName = colNames[icol];
const auto& valueInStates = model.getStateVariableValue(
states[itime], stateName);
const auto& column = table.getDependentColumnAtIndex(icol);
const auto& valueInTable = column[static_cast<int>(itime)];
SimTK_TEST(valueInStates == valueInTable);
}
}
}
void testBoundsCheck() {
Model model("gait2354_simbody.osim");
const auto& state = model.initSystem();
StatesTrajectory states;
states.append(state);
states.append(state);
states.append(state);
states.append(state);
#ifdef NDEBUG
// In DEBUG, Visual Studio puts asserts into the index operator.
states[states.getSize() + 100];
states[4];
states[5];
#endif
SimTK_TEST_MUST_THROW_EXC(states.get(4), IndexOutOfRange);
SimTK_TEST_MUST_THROW_EXC(states.get(states.getSize() + 100),
IndexOutOfRange);
}
void testIntegrityChecks() {
Model arm26("arm26.osim");
const auto& s26 = arm26.initSystem();
Model gait2354("gait2354_simbody.osim");
const auto& s2354 = gait2354.initSystem();
// TODO add models with events, unilateral constraints, etc.
// Times are nondecreasing.
{
StatesTrajectory states;
auto state0(s26);
state0.setTime(0.5);
auto state1(s26);
state1.setTime(0.6);
states.append(state0);
states.append(state1);
SimTK_TEST(states.isConsistent());
SimTK_TEST(states.isNondecreasingInTime());
SimTK_TEST(states.hasIntegrity());
// Users should never do this const cast; it's just for the sake of
// the test.
const_cast<SimTK::State*>(&states[1])->setTime(0.2);
SimTK_TEST(states.isConsistent());
SimTK_TEST(!states.isNondecreasingInTime());
SimTK_TEST(!states.hasIntegrity());
}
// Consistency and compatibility with a model.
{
StatesTrajectory states;
// An empty trajectory is consistent.
SimTK_TEST(states.isConsistent());
// A length-1 trajectory is consistent.
states.append(s26);
SimTK_TEST(states.isConsistent());
// This trajectory is compatible with the arm26 model.
SimTK_TEST(states.isCompatibleWith(arm26));
// Not compatible with gait2354 model.
// Ensures a lower-dimensional trajectory can't pass for a higher
// dimensional model.
SimTK_TEST(!states.isCompatibleWith(gait2354));
// The checks still work with more than 1 state.
states.append(s26);
states.append(s26);
SimTK_TEST(states.isNondecreasingInTime());
SimTK_TEST(states.isConsistent());
SimTK_TEST(states.hasIntegrity());
SimTK_TEST(states.isCompatibleWith(arm26));
SimTK_TEST(!states.isCompatibleWith(gait2354));
}
{
StatesTrajectory states;
states.append(s2354);
// Reverse of the previous check; to ensure that a larger-dimensional
// trajectory can't pass for the smaller dimensional model.
SimTK_TEST(states.isCompatibleWith(gait2354));
SimTK_TEST(!states.isCompatibleWith(arm26));
// Check still works with more than 1 state.
states.append(s2354);
states.append(s2354);
SimTK_TEST(states.isNondecreasingInTime());
SimTK_TEST(states.isConsistent());
SimTK_TEST(states.hasIntegrity());
SimTK_TEST(states.isCompatibleWith(gait2354));
SimTK_TEST(!states.isCompatibleWith(arm26));
}
{
// Cannot append inconsistent states.
StatesTrajectory states;
states.append(s26);
SimTK_TEST_MUST_THROW_EXC(states.append(s2354),
StatesTrajectory::InconsistentState);
// Same check, but swap the models.
StatesTrajectory states2;
states2.append(s2354);
SimTK_TEST_MUST_THROW_EXC(states2.append(s26),
StatesTrajectory::InconsistentState);
}
// TODO Show weakness of the test: two models with the same number of Q's, U's,
// and Z's both pass the check.
}
void testPopulateTrajectoryAndStatesTrajectoryReporter() {
Model model("gait2354_simbody.osim");
// To assist with creating interesting (non-zero) coordinate values:
model.updCoordinateSet().get("pelvis_ty").setDefaultLocked(true);
// Also, test the StatesTrajectoryReporter.
auto* statesCol = new StatesTrajectoryReporter();
statesCol->setName("states_collector_all_steps");
model.addComponent(statesCol);
const double finalTime = 0.05;
{
auto& state = model.initSystem();
SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem());
SimTK::TimeStepper ts(model.getSystem(), integrator);
ts.initialize(state);
ts.setReportAllSignificantStates(true);
integrator.setReturnEveryInternalStep(true);
StatesTrajectory states;
std::vector<double> times;
while (ts.getState().getTime() < finalTime) {
ts.stepTo(finalTime);
times.push_back(ts.getState().getTime());
// StatesTrajectory API for appending states:
states.append(ts.getState());
// For the StatesTrajectoryReporter:
model.getMultibodySystem().realize(ts.getState(), SimTK::Stage::Report);
}
// Make sure we have all the states
SimTK_TEST_EQ((int)states.getSize(), (int)times.size());
SimTK_TEST_EQ((int)statesCol->getStates().getSize(), (int)times.size());
// ...and that they aren't all just references to the same single state.
for (int i = 0; i < (int)states.getSize(); ++i) {
SimTK_TEST_EQ(states[i].getTime(), times[i]);
SimTK_TEST_EQ(statesCol->getStates()[i].getTime(), times[i]);
}
}
// Test the StatesTrajectoryReporter with a constant reporting interval.
statesCol->clear();
auto* statesColInterval = new StatesTrajectoryReporter();
statesColInterval->setName("states_collector_interval");
statesColInterval->set_report_time_interval(0.01);
model.addComponent(statesColInterval);
{
auto& state = model.initSystem();
SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem());
SimTK::TimeStepper ts(model.getSystem(), integrator);
ts.initialize(state);
ts.setReportAllSignificantStates(true);
integrator.setReturnEveryInternalStep(true);
while (ts.getState().getTime() < finalTime) {
ts.stepTo(finalTime);
model.getMultibodySystem().realize(ts.getState(), SimTK::Stage::Report);
}
SimTK_TEST(statesColInterval->getStates().getSize() == 6);
std::vector<double> times { 0, 0.01, 0.02, 0.03, 0.04, 0.05 };
int i = 0;
for (const auto& s : statesColInterval->getStates()) {
ASSERT_EQUAL(s.getTime(), times[i], 1e-5);
++i;
}
}
}
void testNumberOfOrientationsMismatch()
{
cout <<
"\ntestInverseKinematicsSolver::testNumberOfOrientationsMismatch()"
<< endl;
std::unique_ptr<Model> leg{ constructLegWithOrientationFrames() };
const Coordinate& coord = leg->getCoordinateSet()[0];
SimTK::State state = leg->initSystem();
StatesTrajectory states;
// sample time
double dt = 0.1;
int N = 11;
for (int i = 0; i < N; ++i) {
state.updTime() = i*dt;
coord.setValue(state, i*dt*SimTK::Pi / 3);
states.append(state);
}
double err = 0.1;
SimTK::RowVector_<SimTK::Rotation> biases(3, SimTK::Rotation());
// bias thigh_imu
biases[0] *= SimTK::Rotation(err, SimTK::XAxis);
cout << "biases: " << biases << endl;
auto orientationsTable =
generateOrientationsDataFromModelAndStates(*leg,
states,
biases,
0.0,
true);
SimTK::Vector_<SimTK::Rotation> unusedCol(N,
SimTK::Rotation(0.987654321, SimTK::ZAxis));
auto usedOrientationNames = orientationsTable.getColumnLabels();
// add an unused orientation sensor to the given orientation data
orientationsTable.appendColumn("unused", unusedCol);
cout << "Before:\n" << orientationsTable << endl;
// re-order "observed" orientation data
SimTK::Matrix_<SimTK::Rotation> dataGutsCopy
= orientationsTable.getMatrix();
int last = dataGutsCopy.ncol() - 1;
// swap first and last columns
orientationsTable.updMatrix()(0) = dataGutsCopy(last);
orientationsTable.updMatrix()(last) = dataGutsCopy(0);
auto columnNames = orientationsTable.getColumnLabels();
orientationsTable.setColumnLabel(0, columnNames[last]);
orientationsTable.setColumnLabel(last, columnNames[0]);
columnNames = orientationsTable.getColumnLabels();
// Inject NaN in "observations" of thigh_imu orientation data
for (int i = 4; i < 7; ++i) {
orientationsTable.updMatrix()(i, 1).scalarMultiply(SimTK::NaN);
}
cout << "After reorder and NaN injections:\n" << orientationsTable << endl;
OrientationsReference orientationsRef(orientationsTable);
int nmr = orientationsRef.getNumRefs();
auto& osNames = orientationsRef.getNames();
cout << osNames << endl;
MarkersReference mRefs{};
SimTK::Array_<CoordinateReference> coordRefs;
// Reset the initial coordinate value
coord.setValue(state, 0.0);
InverseKinematicsSolver ikSolver(*leg, mRefs, orientationsRef, coordRefs);
double tol = 1e-4;
ikSolver.setAccuracy(tol);
ikSolver.assemble(state);
int nos = ikSolver.getNumOrientationSensorsInUse();
SimTK::Array_<double> orientationErrors(nos);
for (double t : orientationsRef.getTimes()) {
state.updTime() = t;
ikSolver.track(state);
//get the orientation errors
ikSolver.computeCurrentOrientationErrors(orientationErrors);
int nose = orientationErrors.size();
SimTK_ASSERT_ALWAYS(nose == nos,
"InverseKinematicsSolver failed to account "
"for unused orientations reference (observation).");
cout << "time: " << state.getTime() << " |";
auto namesIter = usedOrientationNames.begin();
for (int j = 0; j < nose; ++j) {
const auto& orientationName =
ikSolver.getOrientationSensorNameForIndex(j);
cout << " " << orientationName << " error = " << orientationErrors[j];
SimTK_ASSERT_ALWAYS(*namesIter++ != "unused",
"InverseKinematicsSolver failed to ignore "
"unused orientation reference (observation).");
if (orientationName == "thigh_imu") {//should see error on biased marker
SimTK_ASSERT_ALWAYS(abs(orientationErrors[j]) <= err,
"InverseKinematicsSolver mangled marker order.");
}
else { // other markers should be minimally affected
SimTK_ASSERT_ALWAYS(orientationErrors[j] <= tol,
"InverseKinematicsSolver mangled marker order.");
}
}
cout << endl;
}
}
void testNumberOfMarkersMismatch()
{
cout <<
"\ntestInverseKinematicsSolver::testNumberOfMarkersMismatch()"
<< endl;
std::unique_ptr<Model> pendulum{ constructPendulumWithMarkers() };
const Coordinate& coord = pendulum->getCoordinateSet()[0];
SimTK::State state = pendulum->initSystem();
StatesTrajectory states;
// sample time
double dt = 0.1;
int N = 11;
for (int i = 0; i < N; ++i) {
state.updTime() = i*dt;
coord.setValue(state, i*dt*SimTK::Pi / 3);
states.append(state);
}
double err = 0.05;
SimTK::RowVector_<SimTK::Vec3> biases(3, SimTK::Vec3(0));
// bias m0
biases[0] += SimTK::Vec3(0, err, 0);
cout << "biases: " << biases << endl;
auto markerTable = generateMarkerDataFromModelAndStates(*pendulum,
states,
biases,
0.0,
true);
SimTK::Vector_<SimTK::Vec3> unusedCol(N, SimTK::Vec3(0.987654321));
auto usedMarkerNames = markerTable.getColumnLabels();
// add an unused marker to the marker data
markerTable.appendColumn("unused", unusedCol);
cout << "Before:\n" << markerTable << endl;
// re-order "observed" marker data
SimTK::Matrix_<SimTK::Vec3> dataGutsCopy = markerTable.getMatrix();
int last = dataGutsCopy.ncol() - 1;
// swap first and last columns
markerTable.updMatrix()(0) = dataGutsCopy(last);
markerTable.updMatrix()(last) = dataGutsCopy(0);
auto columnNames = markerTable.getColumnLabels();
markerTable.setColumnLabel(0, columnNames[last]);
markerTable.setColumnLabel(last, columnNames[0]);
columnNames = markerTable.getColumnLabels();
// Inject NaN in "observations" of "mR" marker data
for (int i = 4; i < 7; ++i) {
markerTable.updMatrix()(i, 1) = SimTK::NaN;
}
cout << "After reorder and NaN injections:\n" << markerTable << endl;
MarkersReference markersRef(markerTable);
int nmr = markersRef.getNumRefs();
auto& markerNames = markersRef.getNames();
cout << markerNames << endl;
SimTK::Array_<CoordinateReference> coordRefs;
// Reset the initial coordinate value
coord.setValue(state, 0.0);
InverseKinematicsSolver ikSolver(*pendulum, markersRef, coordRefs);
double tol = 1e-4;
ikSolver.setAccuracy(tol);
ikSolver.assemble(state);
int nm = ikSolver.getNumMarkersInUse();
SimTK::Array_<double> markerErrors(nm);
for (unsigned i = 0; i < markersRef.getNumFrames(); ++i) {
state.updTime() = i*dt;
ikSolver.track(state);
//get the marker errors
ikSolver.computeCurrentMarkerErrors(markerErrors);
int nme = markerErrors.size();
SimTK_ASSERT_ALWAYS(nme == nm,
"InverseKinematicsSolver failed to account "
"for unused marker reference (observation).");
cout << "time: " << state.getTime() << " |";
auto namesIter = usedMarkerNames.begin();
for (int j = 0; j < nme; ++j) {
const auto& markerName = ikSolver.getMarkerNameForIndex(j);
cout << " " << markerName << " error = " << markerErrors[j];
SimTK_ASSERT_ALWAYS( *namesIter++ != "unused",
"InverseKinematicsSolver failed to ignore "
"unused marker reference (observation).");
if (markerName == "m0") {//should see error on biased marker
SimTK_ASSERT_ALWAYS(abs(markerErrors[j]-err) <= tol,
"InverseKinematicsSolver mangled marker order.");
}
else { // other markers should be minimally affected
SimTK_ASSERT_ALWAYS(markerErrors[j] <= tol,
"InverseKinematicsSolver mangled marker order.");
}
}
cout << endl;
}
}
void testTrackWithUpdateMarkerWeights()
{
cout <<
"\ntestInverseKinematicsSolver::testTrackWithUpdateMarkerWeights()"
<< endl;
std::unique_ptr<Model> pendulum{ constructPendulumWithMarkers() };
Coordinate& coord = pendulum->getCoordinateSet()[0];
SimTK::State state = pendulum->initSystem();
StatesTrajectory states;
// sample time
double dt = 0.01;
for (int i = 0; i < 101; ++i) {
state.updTime()=i*dt;
coord.setValue(state, SimTK::Pi / 3);
states.append(state);
}
SimTK::RowVector_<SimTK::Vec3> biases(3, SimTK::Vec3(0));
MarkersReference
markersRef(generateMarkerDataFromModelAndStates(*pendulum,
states, biases,
0.02,
true));
auto& markerNames = markersRef.getNames();
for (const auto& name : markerNames) {
markersRef.updMarkerWeightSet().adoptAndAppend(
new MarkerWeight(name, 1.0));
}
SimTK::Array_<CoordinateReference> coordRefs;
// Reset the initial coordinate value
coord.setValue(state, 0.0);
InverseKinematicsSolver ikSolver(*pendulum, markersRef, coordRefs);
ikSolver.setAccuracy(1e-6);
ikSolver.assemble(state);
SimTK::Array_<double> markerWeights;
markersRef.getWeights(state, markerWeights);
SimTK::Array_<double> leftMarkerWeightedErrors;
double previousErr = 0.1;
for (unsigned i = 0; i < markersRef.getNumFrames(); ++i) {
state.updTime() = i*dt;
// increment the weight of the left marker each time
markerWeights[2] = 0.1*i+1;
ikSolver.updateMarkerWeights(markerWeights);
ikSolver.track(state);
if (i>0 && (i % 10 == 0)) {
//get the marker errors
ikSolver.computeCurrentMarkerErrors(leftMarkerWeightedErrors);
cout << "time: " << state.getTime() << " | " << markerNames[2]
<< "(weight = " << markerWeights[2] << ") error = "
<< leftMarkerWeightedErrors[2] << endl;
// increasing the marker weight (marker[2] = "mL") should cause
// that marker error to decrease
SimTK_ASSERT_ALWAYS(
leftMarkerWeightedErrors[2] < previousErr,
"InverseKinematicsSolver track failed to lower 'left' "
"marker error when marker weight was increased.");
previousErr = leftMarkerWeightedErrors[2];
}
}
}
void testUpdateMarkerWeights()
{
cout << "\ntestInverseKinematicsSolver::testUpdateMarkerWeights()" << endl;
std::unique_ptr<Model> pendulum{ constructPendulumWithMarkers() };
Coordinate& coord = pendulum->getCoordinateSet()[0];
double refVal = 0.123456789;
SimTK::State state = pendulum->initSystem();
coord.setValue(state, refVal);
StatesTrajectory states;
states.append(state);
SimTK::RowVector_<SimTK::Vec3> biases(3, SimTK::Vec3(0));
MarkersReference
markersRef(generateMarkerDataFromModelAndStates(*pendulum,
states, biases,
0.02));
auto& markerNames = markersRef.getNames();
for (const auto& name : markerNames) {
markersRef.updMarkerWeightSet().adoptAndAppend(
new MarkerWeight(name, 1.0));
}
SimTK::Array_<CoordinateReference> coordRefs;
// Reset the initial coordinate value
coord.setValue(state, 0.0);
InverseKinematicsSolver ikSolver(*pendulum, markersRef, coordRefs);
ikSolver.setAccuracy(1.0e-8);
ikSolver.assemble(state);
double coordValue = coord.getValue(state);
cout << "Assembled " << coord.getName() << " value = "
<< coordValue << endl;
SimTK::Array_<double> nominalMarkerErrors;
ikSolver.computeCurrentMarkerErrors(nominalMarkerErrors);
SimTK::Array_<double> markerWeights;
markersRef.getWeights(state, markerWeights);
for (unsigned int i = 0; i < markerNames.size(); ++i) {
cout << markerNames[i] << "(weight = " << markerWeights[i]
<< ") error = " << nominalMarkerErrors[i] << endl;
}
// Increase the weight of the right marker
markerWeights[1] *= 10.0;
ikSolver.updateMarkerWeights(markerWeights);
// Reset the initial coordinate value
coord.setValue(state, 0.0);
ikSolver.track(state);
coordValue = coord.getValue(state);
cout << "Assembled " << coord.getName() << " value = "
<< coordValue << endl;
SimTK::Array_<double> rightMarkerWeightedErrors;
ikSolver.computeCurrentMarkerErrors(rightMarkerWeightedErrors);
for (unsigned int i = 0; i < markerNames.size(); ++i) {
cout << markerNames[i] << "(weight = " << markerWeights[i]
<< ") error = " << rightMarkerWeightedErrors[i] << endl;
}
// increasing the marker weight (marker[1] = "mR") should cause that marker
// error to decrease
SimTK_ASSERT_ALWAYS(rightMarkerWeightedErrors[1] < nominalMarkerErrors[1],
"InverseKinematicsSolver failed to lower 'right' marker error when "
"marker weight was increased.");
// update the marker weights and repeat for the left hand marker "mL"
markerWeights[2] *= 20.0; // "mL"
ikSolver.updateMarkerWeights(markerWeights);
// Reset the initial coordinate value and reassemble
coord.setValue(state, 0.0);
ikSolver.track(state);
coordValue = coord.getValue(state);
cout << "Assembled " << coord.getName() << " value = "
<< coordValue << endl;
SimTK::Array_<double> leftMarkerWeightedErrors;
ikSolver.computeCurrentMarkerErrors(leftMarkerWeightedErrors);
for (unsigned int i = 0; i < markerNames.size(); ++i) {
cout << markerNames[i] << "(weight = " << markerWeights[i]
<< ") error = " << leftMarkerWeightedErrors[i] << endl;
}
// increasing the marker weight (marker[2] = "mL") should cause that marker
// error to decrease
SimTK_ASSERT_ALWAYS(
leftMarkerWeightedErrors[2] < rightMarkerWeightedErrors[2],
"InverseKinematicsSolver failed to lower 'left' marker error when "
"marker weight was increased.");
}
void testAccuracy()
{
cout << "\ntestInverseKinematicsSolver::testAccuracy()" << endl;
std::unique_ptr<Model> pendulum{ constructPendulumWithMarkers() };
Coordinate& coord = pendulum->getCoordinateSet()[0];
double refVal = 0.123456789;
double looseAccuracy = 1e-3;
double tightAccuracy = 1.0e-9;
SimTK::Array_<CoordinateReference> coordRefs;
Constant coordRefFunc(refVal);
CoordinateReference coordRef(coord.getName(), coordRefFunc);
coordRef.setWeight(1.0);
coordRefs.push_back(coordRef);
SimTK::State state = pendulum->initSystem();
double coordValue = coord.getValue(state);
cout.precision(10);
cout << "Initial " << coord.getName() << " value = " << coordValue <<
" referenceValue = " << coordRefs[0].getValue(state) << endl;
coord.setValue(state, refVal);
StatesTrajectory states;
states.append(state);
SimTK::RowVector_<SimTK::Vec3> biases(3, SimTK::Vec3(0));
MarkersReference
markersRef(generateMarkerDataFromModelAndStates(*pendulum, states, biases));
markersRef.setDefaultWeight(1.0);
// Reset the initial coordinate value
coord.setValue(state, 0.0);
InverseKinematicsSolver ikSolver(*pendulum, markersRef, coordRefs);
ikSolver.setAccuracy(looseAccuracy);
ikSolver.assemble(state);
coordValue = coord.getValue(state);
cout << "Assembled " << coord.getName() << " value = " << coordValue << endl;
double accuracy = abs(coordValue - refVal);
cout << "Specified accuracy: " << looseAccuracy << "; achieved: "
<< accuracy << endl;
// verify that the target accuracy was met after assemble()
SimTK_ASSERT_ALWAYS(accuracy <= looseAccuracy,
"InverseKinematicsSolver assemble() failed to meet specified accuracy");
ikSolver.track(state);
coordValue = coord.getValue(state);
cout << "Tracked " << coord.getName() << " value = " << coordValue << endl;
accuracy = abs(coordValue - refVal);
// verify that the target accuracy was met after track()
SimTK_ASSERT_ALWAYS(accuracy <= looseAccuracy,
"InverseKinematicsSolver track() failed to meet specified accuracy");
SimTK::Array_<double> sqMarkerErrors;
double looseSumSqError = 0;
ikSolver.computeCurrentSquaredMarkerErrors(sqMarkerErrors);
for (const double& err : sqMarkerErrors) {
looseSumSqError += err;
}
cout << "For accuracy: " << looseAccuracy << "; Sum-squared Error: "
<< looseSumSqError << endl;
// Reset the initial coordinate value
coord.setValue(state, 0.0);
ikSolver.setAccuracy(tightAccuracy);
// verify that track() throws after changing the accuracy of the Solver
SimTK_TEST_MUST_THROW_EXC(ikSolver.track(state), Exception);
ikSolver.setAccuracy(tightAccuracy);
ikSolver.assemble(state);
coordValue = coord.getValue(state);
cout << "Assembled " << coord.getName() <<" value = "<< coordValue << endl;
accuracy = abs(coord.getValue(state) - refVal);
cout << "Specified accuracy: " << tightAccuracy << "; achieved: "
<< accuracy << endl;
// verify that the target accuracy was met after tightening the accuracy
SimTK_ASSERT_ALWAYS(accuracy <= tightAccuracy,
"InverseKinematicsSolver assemble() failed to meet tightened accuracy");
// perturb the solution to verify that track() achieves the accuracy
coord.setValue(state, refVal-looseAccuracy);
ikSolver.track(state);
accuracy = abs(coord.getValue(state) - refVal);
// verify that track() achieves the tightened accuracy
SimTK_ASSERT_ALWAYS(accuracy <= tightAccuracy,
"InverseKinematicsSolver track() failed to meet tightened accuracy");
double tightSumSqError = 0;
ikSolver.computeCurrentSquaredMarkerErrors(sqMarkerErrors);
for (const double& err : sqMarkerErrors) {
tightSumSqError += err;
}
cout << "For accuracy: " << tightAccuracy << "; Sum-squared Error: "
<< tightSumSqError << endl;
// refining the accuracy should not increase tracking errors
SimTK_ASSERT_ALWAYS(tightSumSqError <= looseSumSqError,
"InverseKinematicsSolver failed to maintain or lower marker errors "
"when accuracy was tightened.");
}
