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."); }