/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 *
 * This method should be overridden in the child class.  It is
 * included here so that the child class will not have to implement it if it
 * is not necessary.
 *
 * @param state system State
 * @param aStep Step number of the integration.
 *
 * @return -1 on error, 0 otherwise.
 */
int ForceReporter::
begin(SimTK::State& s)
{
    if(!proceed()) return(0);

    tidyForceNames();
    // LABELS
    constructColumnLabels(s);
    // RESET STORAGE
    _forceStore.reset(s.getTime());

    // RECORD
    int status = 0;
    if(_forceStore.getSize()<=0) {
        status = record(s);
    }

    return(status);
}

/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 * This method is meant to be called at the begining of an integration in
 * Model::integBeginCallback() and has the same argument list.
 *
 * @param s SimTK:State
 *
 * @return -1 on error, 0 otherwise.
 */
int InducedAccelerations::begin(SimTK::State &s)
{
	if(!proceed()) return(0);

	initialize(s);

	// RESET STORAGES
	for(int i = 0; i<_storeInducedAccelerations.getSize(); i++){
		_storeInducedAccelerations[i]->reset(s.getTime());
	}

	cout << "Performing Induced Accelerations Analysis" << endl;

	// RECORD
	int status = 0;
	status = record(s);

	return(status);
}

/**
 * Record the ForceReporter quantities.
 */
int ForceReporter::record(const SimTK::State& s)
{
    if(_model==NULL) return(-1);

    // MAKE SURE ALL ForceReporter QUANTITIES ARE VALID
    _model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics );

    StateVector nextRow = StateVector(s.getTime());

    // NUMBER OF Forces
    const ForceSet& forces = _model->getForceSet(); // This does not contain gravity
    int nf = forces.getSize();

    for(int i=0;i<nf;i++) {
        // If body force we need to record six values for torque+force
        // If muscle we record one scalar
        OpenSim::Force& nextForce = (OpenSim::Force&)forces[i];
        if (nextForce.isDisabled(s)) continue;
        Array<double> values = nextForce.getRecordValues(s);
        nextRow.getData().append(values);
    }

    if(_includeConstraintForces){
        // NUMBER OF Constraints
        const ConstraintSet& constraints = _model->getConstraintSet(); // This does not contain gravity
        int nc = constraints.getSize();

        for(int i=0;i<nc;i++) {
            OpenSim::Constraint& nextConstraint = (OpenSim::Constraint&)constraints[i];
            if (nextConstraint.isDisabled(s)) continue;
            Array<double> values = nextConstraint.getRecordValues(s);
            nextRow.getData().append(values);
        }
    }
    _forceStore.append(nextRow);

    return(0);
}

/**
* Record the actuation quantities.
*/
int Actuation::
record(const SimTK::State& s)
{
    if (_model == NULL) return(-1);

    // MAKE SURE ALL ACTUATION QUANTITIES ARE VALID
    _model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics);

    // TIME NORMALIZATION
    double tReal = s.getTime();

    // FORCE
    const Set<Actuator>& fSet = _model->getActuators();
    for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        if (!fSet.get(i).get_isDisabled())
            _fsp[iact++] = act->getActuation(s);
    }
    _forceStore->append(tReal, _na, _fsp);

    // SPEED
    for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        if (!fSet.get(i).get_isDisabled())
            _fsp[iact++] = act->getSpeed(s);
    }
    _speedStore->append(tReal, _na, _fsp);

    // POWER
    for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
        if (!fSet.get(i).get_isDisabled())
            _fsp[iact++] = fSet.get(i).getPower(s);
    }
    _powerStore->append(tReal, _na, _fsp);


    return(0);
}

/**
 * Record the kinematics.
 */
int PointKinematics::
record(const SimTK::State& s)
{
    const SimbodyEngine& de = _model->getSimbodyEngine();

    // VARIABLES
    SimTK::Vec3 vec;

    const double& time = s.getTime();

    // POSITION
    de.getPosition(s, *_body,_point,vec);
    if(_relativeToBody){
        de.transformPosition(s, _model->getGround(), vec, *_relativeToBody, vec);
    }

    _pStore->append(time, vec);

    // VELOCITY
    de.getVelocity(s, *_body,_point,vec);
    if(_relativeToBody){
        de.transform(s, _model->getGround(), vec, *_relativeToBody, vec);
    }

    _vStore->append(time, vec);

    // ACCELERATIONS
    _model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration);
    de.getAcceleration(s, *_body,_point,vec);
    if(_relativeToBody){
        de.transform(s, _model->getGround(), vec, *_relativeToBody, vec);
    }

    _aStore->append(time, vec);

    return(0);
}

/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 * This method is meant to be called at the beginning of an integration 
 *
 * @param s Current state .
 *
 * @return -1 on error, 0 otherwise.
 */
int StaticOptimization::
begin(SimTK::State& s )
{
    if(!proceed()) return(0);

    // Make a working copy of the model
    delete _modelWorkingCopy;
    _modelWorkingCopy = _model->clone();
    _modelWorkingCopy->initSystem();

    // Replace model force set with only generalized forces
    if(_model) {
        SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->updWorkingState();
        // Update the _forceSet we'll be computing inverse dynamics for
        if(_ownsForceSet) delete _forceSet;
        if(_useModelForceSet) {
            // Set pointer to model's internal force set
            _forceSet = &_modelWorkingCopy->updForceSet();
            _ownsForceSet = false;
        } else {
            ForceSet& as = _modelWorkingCopy->updForceSet();
            // Keep a copy of forces that are not muscles to restore them back.
            ForceSet* saveForces = as.clone();
            // Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom
            _forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false);
            _ownsForceSet = false;
            _modelWorkingCopy->setAllControllersEnabled(false);
            _numCoordinateActuators = _forceSet->getSize();
            // Copy whatever forces that are not muscles back into the model
            
            for(int i=0; i<saveForces->getSize(); i++){
                const Force& f=saveForces->get(i);
                if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL)
                    as.append(saveForces->get(i).clone());
            }
        }

        SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();
        // Set modeling options for Actuators to be overriden
        for(int i=0; i<_forceSet->getSize(); i++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
            if( act ) {
                act->overrideActuation(sWorkingCopy, true);
            }
        }

        sWorkingCopy.setTime(s.getTime());
        sWorkingCopy.setQ(s.getQ());
        sWorkingCopy.setU(s.getU());
        sWorkingCopy.setZ(s.getZ());
        _modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Velocity);
        _modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
        // Gather indices into speed set corresponding to the unconstrained degrees of freedom 
        // (for which we will set acceleration constraints)
        _accelerationIndices.setSize(0);
        const CoordinateSet& coordSet = _model->getCoordinateSet();
        for(int i=0; i<coordSet.getSize(); i++) {
            const Coordinate& coord = coordSet.get(i);
            if(!coord.isConstrained(sWorkingCopy)) {
                Array<int> inds = _statesStore->
                    getColumnIndicesForIdentifier(coord.getName()) ;
                _accelerationIndices.append(inds[0]);
            }
        }

        int na = _forceSet->getSize();
        int nacc = _accelerationIndices.getSize();

        if(na < nacc) 
            throw(Exception("StaticOptimization: ERROR- over-constrained "
                "system -- need at least as many forces as there are degrees of freedom.\n") );

        _forceReporter.reset(new ForceReporter(_modelWorkingCopy));
        _forceReporter->begin(sWorkingCopy);
        _forceReporter->updForceStorage().reset();

        _parameters.resize(_modelWorkingCopy->getNumControls());
        _parameters = 0;
    }

    _statesSplineSet=GCVSplineSet(5,_statesStore);

    // DESCRIPTION AND LABELS
    constructDescription();
    constructColumnLabels();

    deleteStorage();
    allocateStorage();

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

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

bool Manager::doIntegration(SimTK::State& s, int step, double dtFirst ) {

    if(!_integ) {
        throw Exception("Manager::doIntegration(): "
                "Integrator has not been set. Construct the Manager "
                "with an integrator, or call Manager::setIntegrator().");
    }

    // CLEAR ANY INTERRUPT
    // Halts must arrive during an integration.
    clearHalt();

    double dt/*,dtPrev*/,tReal;
    double time =_ti;
    dt=dtFirst;
    if(dt>_dtMax) dt = _dtMax;
    //dtPrev=dt;

    // CHECK SPECIFIED DT STEPPING
    
    if(_specifiedDT) {
        if(_tArray.getSize()<=0) {
            string msg="IntegRKF.integrate: ERR- specified dt stepping not";
            msg += "possible-- empty time array.";
            throw( Exception(msg) );
        }
        double first = _tArray[0];
        double last = _tArray.getLast();
        if((getTimeArrayStep(_ti)<0) || (_ti<first) || (_tf>last)) {
            string msg="IntegRKF.integrate: ERR- specified dt stepping not";
            msg +="possible-- time array does not cover the requested";
            msg +=" integration interval.";
            throw(Exception(msg));
        }
    }

    // RECORD FIRST TIME STEP
    if(!_specifiedDT) {
        resetTimeAndDTArrays(time);
        if(_tArray.getSize()<=0) {
            _tArray.append(time);
        }
    }
    bool fixedStep = false;
    double fixedStepSize;
    if( _constantDT || _specifiedDT) fixedStep = true;

    // If _system is has been set we should be integrating a CMC system
    // not the model's system.
    const SimTK::System& sys = _system ? *_system 
                                       : _model->getMultibodySystem();

    // Only initialize a TimeStepper if it hasn't been done yet
    if (_timeStepper == NULL) initializeTimeStepper(sys, s);

    SimTK::Integrator::SuccessfulStepStatus status;

    if( fixedStep ) {
        dt = getFixedStepSize(getTimeArrayStep(_ti));
    } else {
        _integ->setReturnEveryInternalStep(true); 
    }

    if( s.getTime()+dt >= _tf ) dt = _tf - s.getTime();
   
    // We need to be at a valid stage to initialize the controls, but only when 
    // we are integrating the complete model system, not the CMC system. This 
    // is very ugly and a cleaner solution is required- aseth
    if(_system == NULL)
        sys.realize(s, SimTK::Stage::Velocity); // this is multibody system 
    initialize(s, dt);  

    if( fixedStep){
        s.updTime() = time;
        sys.realize(s, SimTK::Stage::Acceleration);

        if(_performAnalyses)_model->updAnalysisSet().step(s, step);
        tReal = s.getTime();
        if( _writeToStorage ) {
            SimTK::Vector stateValues = _model->getStateVariableValues(s);
            StateVector vec;
            vec.setStates(tReal, stateValues);
            getStateStorage().append(vec);
            if(_model->isControlled())
                _controllerSet->storeControls(s,step);
        }
    }

    double stepToTime = _tf;

    // LOOP
    while( time  < _tf ) {
        if( fixedStep ){
              fixedStepSize = getNextTimeArrayTime( time ) - time;
             if( fixedStepSize + time  >= _tf )  fixedStepSize = _tf - time;
             _integ->setFixedStepSize( fixedStepSize );
             stepToTime = time + fixedStepSize; 
        }

        // stepTo() does not return if it fails. However, the final step
//.........这里部分代码省略.........

/**
 * Record the results.
 */
int StaticOptimization::
record(const SimTK::State& s)
{
    if(!_modelWorkingCopy) return -1;

    // Set model to whatever defaults have been updated to from the last iteration
    SimTK::State& sWorkingCopy = _modelWorkingCopy->updWorkingState();
    sWorkingCopy.setTime(s.getTime());
    _modelWorkingCopy->initStateWithoutRecreatingSystem(sWorkingCopy); 

    // update Q's and U's
    sWorkingCopy.setQ(s.getQ());
    sWorkingCopy.setU(s.getU());

    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
    //_modelWorkingCopy->equilibrateMuscles(sWorkingCopy);

    const Set<Actuator>& fs = _modelWorkingCopy->getActuators();

    int na = fs.getSize();
    int nacc = _accelerationIndices.getSize();

    // IPOPT
    _numericalDerivativeStepSize = 0.0001;
    _optimizerAlgorithm = "ipopt";
    _printLevel = 0;
    //_optimizationConvergenceTolerance = 1e-004;
    //_maxIterations = 2000;

    // Optimization target
    _modelWorkingCopy->setAllControllersEnabled(false);
    StaticOptimizationTarget target(sWorkingCopy,_modelWorkingCopy,na,nacc,_useMusclePhysiology);
    target.setStatesStore(_statesStore);
    target.setStatesSplineSet(_statesSplineSet);
    target.setActivationExponent(_activationExponent);
    target.setDX(_numericalDerivativeStepSize);

    // Pick optimizer algorithm
    SimTK::OptimizerAlgorithm algorithm = SimTK::InteriorPoint;
    //SimTK::OptimizerAlgorithm algorithm = SimTK::CFSQP;

    // Optimizer
    SimTK::Optimizer *optimizer = new SimTK::Optimizer(target, algorithm);

    // Optimizer options
    //cout<<"\nSetting optimizer print level to "<<_printLevel<<".\n";
    optimizer->setDiagnosticsLevel(_printLevel);
    //cout<<"Setting optimizer convergence criterion to "<<_convergenceCriterion<<".\n";
    optimizer->setConvergenceTolerance(_convergenceCriterion);
    //cout<<"Setting optimizer maximum iterations to "<<_maximumIterations<<".\n";
    optimizer->setMaxIterations(_maximumIterations);
    optimizer->useNumericalGradient(false);
    optimizer->useNumericalJacobian(false);
    if(algorithm == SimTK::InteriorPoint) {
        // Some IPOPT-specific settings
        optimizer->setLimitedMemoryHistory(500); // works well for our small systems
        optimizer->setAdvancedBoolOption("warm_start",true);
        optimizer->setAdvancedRealOption("obj_scaling_factor",1);
        optimizer->setAdvancedRealOption("nlp_scaling_max_gradient",1);
    }

    // Parameter bounds
    SimTK::Vector lowerBounds(na), upperBounds(na);
    for(int i=0,j=0;i<fs.getSize();i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fs.get(i));
        if (act) {
            lowerBounds(j) = act->getMinControl();
            upperBounds(j) = act->getMaxControl();
            j++;
        }
    }
    
    target.setParameterLimits(lowerBounds, upperBounds);

    _parameters = 0; // Set initial guess to zeros

    // Static optimization
    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy,SimTK::Stage::Velocity);
    target.prepareToOptimize(sWorkingCopy, &_parameters[0]);

    //LARGE_INTEGER start;
    //LARGE_INTEGER stop;
    //LARGE_INTEGER frequency;

    //QueryPerformanceFrequency(&frequency);
    //QueryPerformanceCounter(&start);

    try {
        target.setCurrentState( &sWorkingCopy );
        optimizer->optimize(_parameters);
    }
    catch (const SimTK::Exception::Base& ex) {
        cout << ex.getMessage() << endl;
        cout << "OPTIMIZATION FAILED..." << endl;
        cout << endl;
        cout << "StaticOptimization.record:  WARN- The optimizer could not find a solution at time = " << s.getTime() << endl;
        cout << endl;
//.........这里部分代码省略.........

/**
 * Record the inverse dynamics forces.
 */
int InverseDynamics::
record(const SimTK::State& s)
{
    if(!_modelWorkingCopy) return -1;

//cout << "\nInverse Dynamics record() : \n" << endl;
    // Set model Q's and U's
    SimTK::State sWorkingCopy = _modelWorkingCopy->getWorkingState();

    // Set modeling options for Actuators to be overridden
    for(int i=0; i<_forceSet->getSize(); i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
        if( act ) {
            act->overrideActuation(sWorkingCopy, true);
        }
    }

    // Having updated the model, at least re-realize Model stage!
    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Model);

    sWorkingCopy.setTime(s.getTime());
    sWorkingCopy.setQ(s.getQ());
    sWorkingCopy.setU(s.getU());


    // Having updated the states, at least realize to velocity!
    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);

    int nf = _numCoordinateActuators;
    int nacc = _accelerationIndices.getSize();
    // int nq = _modelWorkingCopy->getNumCoordinates();

//cout << "\nQ= " << s.getQ() << endl;
//cout << "\nU= " << s.getU() << endl;
    // Build linear constraint matrix and constant constraint vector
    SimTK::Vector f(nf), c(nacc);
    f = 0;
    computeAcceleration(sWorkingCopy, &f[0], &_constraintVector[0]);

    for(int j=0; j<nf; j++) {
        f[j] = 1;
        computeAcceleration(sWorkingCopy, &f[0], &c[0]);
        for(int i=0; i<nacc; i++) _constraintMatrix(i,j) = (c[i] - _constraintVector[i]);
        f[j] = 0;
    }

    auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();

    for(int i=0; i<nacc; i++) {
        const Coordinate& coord = *coordinates[_accelerationIndices[i]];
        int ind = _statesStore->getStateIndex(coord.getSpeedName(), 0);
        if (ind < 0){
            // get the full coordinate speed state variable path name
            string fullname = coord.getStateVariableNames()[1];
            ind = _statesStore->getStateIndex(fullname, 0);
            if (ind < 0){
                string msg = "InverseDynamics::record(): \n";
                msg += "target motion for coordinate '";
                msg += coord.getName() + "' not found.";
                throw Exception(msg);
            }
        }
        Function& targetFunc = _statesSplineSet.get(ind);
        std::vector<int> firstDerivComponents(1);
        firstDerivComponents[0]=0;
        double targetAcceleration = targetFunc.calcDerivative(firstDerivComponents, SimTK::Vector(1, sWorkingCopy.getTime()));
//cout <<  coord.getName() << " t=" << sWorkingCopy.getTime() << "  acc=" << targetAcceleration << " index=" << _accelerationIndices[i] << endl; 
        _constraintVector[i] = targetAcceleration - _constraintVector[i];
    }
    //cout << "NEW Constraint Vector Adjusted = " << endl;
    //_constraintVector.dump(&t);

    // LAPACK SOLVER
    // NOTE: It destroys the matrices/vectors we pass to it, so we need to pass it copies of performanceMatrix and performanceVector (don't bother making
    // copies of _constraintMatrix/Vector since those are reinitialized each time anyway)
    int info;
    SimTK::Matrix performanceMatrixCopy = _performanceMatrix;
    SimTK::Vector performanceVectorCopy = _performanceVector;
//cout << "performanceMatrixCopy : " << performanceMatrixCopy << endl;
//cout << "performanceVectorCopy : " << performanceVectorCopy << endl;
//cout << "_constraintMatrix : " << _constraintMatrix << endl;
//cout << "_constraintVector : " << _constraintVector << endl;
//cout << "nf=" << nf << "  nacc=" << nacc << endl;
    dgglse_(nf, nf, nacc, &performanceMatrixCopy(0,0), nf, &_constraintMatrix(0,0), nacc, &performanceVectorCopy[0], &_constraintVector[0], &f[0], &_lapackWork[0], _lapackWork.size(), info);

    // Record inverse dynamics forces
    _storage->append(sWorkingCopy.getTime(),nf,&f[0]);

//cout << "\n ** f : " << f << endl << endl;

    return 0;
}

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

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

/** get the value of the CoordinateReference */
double CoordinateReference::getValue(const SimTK::State &s) const
{
    SimTK::Vector t(1, s.getTime());
    return _coordinateValueFunction->calcValue(t);
}

    void IKSolverParallel::operator()(){

        SimTK::State s = model_.initSystem();
        bool localRunCondition(true);
        std::vector<double> sortedMarkerWeights;
        for (auto it : markerNames_)
            sortedMarkerWeights.push_back(markerWeights_[it]);
        //I may need to use a raw pointer because of OpenSim..
        unique_ptr<MarkersReferenceFromQueue> markerReference(new MarkersReferenceFromQueue(inputThreadPoolJobs_, markerNames_, sortedMarkerWeights));

        OpenSim::Set<OpenSim::MarkerWeight> osimMarkerWeights;
        for (auto it : markerNames_)
            osimMarkerWeights.adoptAndAppend(new OpenSim::MarkerWeight(it, markerWeights_[it]));
        markerReference->setMarkerWeightSet(osimMarkerWeights);
        SimTK::Array_<OpenSim::CoordinateReference> coordinateRefs;

        double previousTime(0.);
        double currentTime(0.);
        OpenSim::InverseKinematicsSolver ikSolver(model_, *markerReference, coordinateRefs, contraintWeight_);
        ikSolver.setAccuracy(sovlerAccuracy_);
        doneWithSubscriptions_.wait();
        bool isAssembled(false);
        while (!isAssembled) {
            try {

                ikSolver.assemble(s);
                isAssembled = true;
            }
            catch (...){
                std::cerr << "Time " << s.getTime() << " Model not assembled" << std::endl;
                markerReference->purgeCurrentFrame();
                isAssembled = false;
            }
        }

        SimTK::State defaultState(s);
        currentTime = markerReference->getCurrentTime();
        s.updTime() = currentTime;
        previousTime = currentTime;
        pushState(s);
        unsigned ct = 0;
        //     auto start = std::chrono::system_clock::now();
        //init the stats, so we can start measuring the frame processing time correctly

        while (localRunCondition) {
            if (!markerReference->isEndOfData()){
                try{
                    stopWatch_.init();
                    ikSolver.track(s);
                    stopWatch_.log();
                    if (!isWithinRom(s))
                        s = defaultState;
                }
                catch (...) {
                    s = defaultState;
                }
                SimTK::Array_<double> markerErrors;
                ikSolver.computeCurrentMarkerErrors(markerErrors);
                currentTime = markerReference->getCurrentTime();
                s.updTime() = currentTime;
                previousTime = currentTime;
                pushState(s);
                defaultState = s;
                ++ct;
            }
            else {
                localRunCondition = false;
                outputGeneralisedCoordinatesQueue_.push(rtosim::EndOfData::get<GeneralisedCoordinatesFrame>());
            }
        }
#ifdef RTOSIM_DEBUG
        cout << " IKSolver " << std::this_thread::get_id() << " is closing\n";
#endif
        doneWithExecution_.wait();
    }

/**
 * Compute and record the results.
 *
 * This method, for the purpose of example, records the position and
 * orientation of each body in the model.  You will need to customize it
 * to perform your analysis.
 *
 * @param aT Current time in the simulation.
 * @param aX Current values of the controls.
 * @param aY Current values of the states: includes generalized coords and speeds
 */
int InducedAccelerations::record(const SimTK::State& s)
{
	int nu = _model->getNumSpeeds();
	double aT = s.getTime();
	cout << "time = " << aT << endl;

	SimTK::Vector Q = s.getQ();

	// Reset Accelerations for coordinates at this time step
	for(int i=0;i<_coordSet.getSize();i++) {
		_coordIndAccs[i]->setSize(0);
	}

	// Reset Accelerations for bodies at this time step
	for(int i=0;i<_bodySet.getSize();i++) {
		_bodyIndAccs[i]->setSize(0);
	}

	// Reset Accelerations for system center of mass at this time step
	_comIndAccs.setSize(0);
	_constraintReactions.setSize(0);

    SimTK::State s_analysis = _model->getWorkingState();

	_model->initStateWithoutRecreatingSystem(s_analysis);
	// Just need to set current time and position to determine state of constraints
	s_analysis.setTime(aT);
	s_analysis.setQ(Q);

	// Check the external forces and determine if contact constraints should be applied at this time
	// and turn constraint on if it should be.
	Array<bool> constraintOn = applyContactConstraintAccordingToExternalForces(s_analysis);

	// Hang on to a state that has the right flags for contact constraints turned on/off
	_model->setPropertiesFromState(s_analysis);
	// Use this state for the remainder of this step (record)
	s_analysis = _model->getMultibodySystem().realizeTopology();
	// DO NOT recreate the system, will lose location of constraint
	_model->initStateWithoutRecreatingSystem(s_analysis);

	// Cycle through the force contributors to the system acceleration
	for(int c=0; c< _contributors.getSize(); c++){			
		//cout << "Solving for contributor: " << _contributors[c] << endl;
		// Need to be at the dynamics stage to disable a force
		_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Dynamics);
		
		if(_contributors[c] == "total"){
			// Set gravity ON
			_model->getGravityForce().enable(s_analysis);

			//Use same conditions on constraints
			s_analysis.setTime(aT);
			// Set the configuration (gen. coords and speeds) of the model.
			s_analysis.setQ(Q);
			s_analysis.setU(s.getU());
			s_analysis.setZ(s.getZ());

			//Make sure all the actuators are on!
			for(int f=0; f<_model->getActuators().getSize(); f++){
				_model->updActuators().get(f).setDisabled(s_analysis, false);
			}

			// Get to  the point where we can evaluate unilateral constraint conditions
			 _model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);

		    /* *********************************** ERROR CHECKING *******************************
			SimTK::Vec3 pcom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterLocationInGround(s_analysis);
			SimTK::Vec3 vcom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterVelocityInGround(s_analysis);
			SimTK::Vec3 acom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_analysis);

			SimTK::Matrix M;
			_model->getMultibodySystem().getMatterSubsystem().calcM(s_analysis, M);
			cout << "mass matrix: " << M << endl;

			SimTK::Inertia sysInertia = _model->getMultibodySystem().getMatterSubsystem().calcSystemCentralInertiaInGround(s_analysis);
			cout << "system inertia: " << sysInertia << endl;

			SimTK::SpatialVec sysMomentum =_model->getMultibodySystem().getMatterSubsystem().calcSystemMomentumAboutGroundOrigin(s_analysis);
			cout << "system momentum: " << sysMomentum << endl;

			const SimTK::Vector &appliedMobilityForces = _model->getMultibodySystem().getMobilityForces(s_analysis, SimTK::Stage::Dynamics);
			appliedMobilityForces.dump("All Applied Mobility Forces");
		
			// Get all applied body forces like those from conact
			const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces = _model->getMultibodySystem().getRigidBodyForces(s_analysis, SimTK::Stage::Dynamics);
			appliedBodyForces.dump("All Applied Body Forces");

			SimTK::Vector ucUdot;
			SimTK::Vector_<SimTK::SpatialVec> ucA_GB;
//.........这里部分代码省略.........

 SimTK::Vector getQ(const SimTK::State& state) const {
     return state.getQ();
 }

int main()
{
  try {
    int  status = 0;

    // To Retrace the steps taken by the GUI this test case follows the same call sequence:
    // new Model(file)
    // new OpenSimContext(model.initSystem(), model);
    // context.updateDisplayer(muscle)  // to display muscles
    // context.getCurrentPath(muscle)
    // context.getTransform(body)
    // context.transformPosition(body, loc, global)  // to display markers
    // context.getLocked(Coordinate)
    // context.getValue(cooridnate)
    LoadOpenSimLibrary("osimActuators");
    LoadOpenSimLibrary("osimSimulation");
    LoadOpenSimLibrary("osimJavaJNI");

    Model *model = new Model("wrist.osim");
    OpenSimContext* context = new OpenSimContext(&model->initSystem(), model);
    const ForceSet& fs = model->getForceSet();
    int n1 = fs.getNumGroups();
    const ObjectGroup* grp = fs.getGroup("wrist");
    assert(grp);
    const Array<const Object*>& members = grp->getMembers();
    int sz = members.getSize();
    ASSERT_EQUAL(sz,5,0);
    assert(members.get(0)->getName()=="ECRB");
    delete model;
    delete context;
    model = new Model("arm26_20.osim");
    context = new OpenSimContext(&model->initSystem(), model);
    // Make a copy of state contained in context ad make sure content match 
    SimTK::State stateCopy = context->getCurrentStateCopy();
    assert(context->getCurrentStateRef().toString()==stateCopy.toString());

    Array<std::string> stateNames = model->getStateVariableNames();
    OpenSim::Force* dForce=&(model->updForceSet().get("TRIlong"));
    Muscle* dTRIlong = dynamic_cast<Muscle*>(dForce);
    assert(dTRIlong);
    context->setPropertiesFromState();
    OpenSim::Thelen2003Muscle* thelenMsl = dynamic_cast<Thelen2003Muscle*>(dTRIlong);
    AbstractProperty& dProp = thelenMsl->updPropertyByName("ignore_tendon_compliance");
    
    PropertyHelper::setValueBool(true, dProp);
    cout << "Prop after is " << dProp.toString() << endl;

    bool exceptionThrown = false;
    try{// adding to the system should cause Muscle that do not handle
        // ignore_tendon_compliance to throw an exception
        context->recreateSystemKeepStage();
    }   
    catch (const std::exception& e) {
        cout << e.what() << endl;
        exceptionThrown = true;
        PropertyHelper::setValueBool(false, dProp);
        cout << "Prop reset to " << dProp.toString() << endl;
        // recreate the system so test can continue
        context->recreateSystemKeepStage();
    }

    SimTK_ASSERT_ALWAYS(exceptionThrown, "Setting ignore_tendon_compliance must throw an exception.");


    AbstractProperty& dProp2 = thelenMsl->updPropertyByName("ignore_tendon_compliance");
    cout << "Prop after create system is " << dProp2.toString() << endl;
    bool after = PropertyHelper::getValueBool(dProp);
    SimTK_ASSERT_ALWAYS(!after, "Property has wrong value!!");
    dTRIlong->updGeometryPath().updateGeometry(context->getCurrentStateRef());
    const OpenSim::Array<PathPoint*>& path = context->getCurrentPath(*dTRIlong);
    cout << "Muscle Path" << endl;
    cout << path.getSize() << endl;
    for(int i=0; i< path.getSize(); i++)
        cout << path.get(i)->getBodyName() << path.get(i)->getLocation() << endl;
    // Compare to known path 
    const OpenSim::Body& dBody = model->getBodySet().get("r_ulna_radius_hand");
    Transform xform = context->getTransform(dBody);
    cout << xform << endl;
    double flat[16];
    context->getTransformAsDouble16(xform, flat);
    // Compare to known xform
    double markerPosition[] = {.005000000000, -0.290400000000, 0.030000000000};
    double markerPositionInGround[3];
    context->transformPosition(dBody, markerPosition, markerPositionInGround);  // to display markers
    cout << "Global frame position = " << markerPositionInGround[0] <<  
        markerPositionInGround[1] << markerPositionInGround[2]<< endl;
    // Check xformed point against known position
    const Coordinate& dr_elbow_flex = model->getCoordinateSet().get("r_elbow_flex");
    bool isLocked = context->getLocked(dr_elbow_flex);
    assert(!isLocked);
    double startValue = context->getValue(dr_elbow_flex);
    cout << "Coordinate start value = " << startValue << endl;
    double length1 = context->getMuscleLength(*dTRIlong);
    cout << length1 << endl