C++ StatesTrajectory::append方法代码示例

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

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

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 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];

//.........这里部分代码省略.........

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
//.........这里部分代码省略.........

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 "
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........