C++ State::getQ方法代码示例

/**
 * Assemble the model such that it satisfies configuration goals and constraints
 * The input state is used to initialize the assembly and then is updated to 
 * return the resulting assembled configuration.
 */
void AssemblySolver::assemble(SimTK::State &state)
{
    // Make a working copy of the state that will be used to set the internal 
    // state of the solver. This is necessary because we may wish to disable 
    // redundant constraints, but do not want this  to effect the state of 
    // constraints the user expects
    SimTK::State s = state;
    
    // Make sure goals are up-to-date.
    setupGoals(s);

    // Let assembler perform some internal setup
    _assembler->initialize(s);
    
    /* TODO: Useful to include through debug message/log in the future
    printf("UNASSEMBLED CONFIGURATION (normerr=%g, maxerr=%g, cost=%g)\n",
        _assembler->calcCurrentErrorNorm(),
        max(abs(_assembler->getInternalState().getQErr())),
        _assembler->calcCurrentGoal());
    cout << "Model numQs: " << _assembler->getInternalState().getNQ() 
        << " Assembler num freeQs: " << _assembler->getNumFreeQs() << endl;
    */
    try{
        // Now do the assembly and return the updated state.
        _assembler->assemble();
        // Update the q's in the state passed in
        _assembler->updateFromInternalState(s);
        state.updQ() = s.getQ();
        state.updU() = s.getU();

        // Get model coordinates
        const CoordinateSet& modelCoordSet = getModel().getCoordinateSet();
        // Make sure the locks in original state are restored
        for(int i=0; i< modelCoordSet.getSize(); ++i){
            bool isLocked = modelCoordSet[i].getLocked(state);
            if(isLocked)
                modelCoordSet[i].setLocked(state, isLocked);
        }
        /* TODO: Useful to include through debug message/log in the future
        printf("ASSEMBLED CONFIGURATION (acc=%g tol=%g normerr=%g, maxerr=%g, cost=%g)\n",
            _assembler->getAccuracyInUse(), _assembler->getErrorToleranceInUse(), 
            _assembler->calcCurrentErrorNorm(), max(abs(_assembler->getInternalState().getQErr())),
            _assembler->calcCurrentGoal());
        printf("# initializations=%d\n", _assembler->getNumInitializations());
        printf("# assembly steps: %d\n", _assembler->getNumAssemblySteps());
        printf(" evals: goal=%d grad=%d error=%d jac=%d\n",
            _assembler->getNumGoalEvals(), _assembler->getNumGoalGradientEvals(),
            _assembler->getNumErrorEvals(), _assembler->getNumErrorJacobianEvals());
        */
    }
    catch (const std::exception& ex)
    {
        std::string msg = "AssemblySolver::assemble() Failed: ";
        msg += ex.what();
        throw Exception(msg);
    }
}

    void IKSolverParallel::pushState(const SimTK::State& s) {

        GeneralisedCoordinatesData currentData(nCoordinates_);
        std::vector<double> q(nCoordinates_);
        SimTK::Vector stateQ(s.getQ());
        for (unsigned i(0); i < nCoordinates_; ++i)
            q[i] = stateQ[i];
        currentData.setQ(q);
        outputGeneralisedCoordinatesQueue_.push({ s.getTime(), currentData });
    }

    bool IKSolverParallel::isWithinRom(const SimTK::State& s) const {

        bool isInRom(true);

        auto q(s.getQ());
        for (unsigned i(0); i < nCoordinates_; ++i) {
            auto rangeMax(model_.getCoordinateSet().get(i).getRangeMax());
            auto rangeMin(model_.getCoordinateSet().get(i).getRangeMin());

            if (q[i] > rangeMax || q[i] < rangeMin) {
                isInRom = false;
                std::cout << coordinateNames_[i] << " is outside its range of motion" << std::endl;
            }
        }
        return isInRom;
    }

/**
 * 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;
}

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

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

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

/**
 * Compute the controls for a simulation.
 *
 * This method alters the control set in order to control the simulation.
 */
void CMC::
computeControls(SimTK::State& s, ControlSet &controlSet)
{
    // CONTROLS SHOULD BE RECOMPUTED- NEED A NEW TARGET TIME
    _tf = s.getTime() + _targetDT;

    int i,j;

    // TURN ANALYSES OFF
    _model->updAnalysisSet().setOn(false);

    // TIME STUFF
    double tiReal = s.getTime(); 
    double tfReal = _tf; 

    cout<<"CMC.computeControls:  t = "<<s.getTime()<<endl;
    if(_verbose) { 
        cout<<"\n\n----------------------------------\n";
        cout<<"integration step size = "<<_targetDT<<",  target time = "<<_tf<<endl;
    }

    // SET CORRECTIONS 
    int nq = _model->getNumCoordinates();
    int nu = _model->getNumSpeeds();
    FunctionSet *qSet = _predictor->getCMCActSubsys()->getCoordinateTrajectories();
    FunctionSet *uSet = _predictor->getCMCActSubsys()->getSpeedTrajectories();
    Array<double> qDesired(0.0,nq),uDesired(0.0,nu);
    qSet->evaluate(qDesired,0,tiReal);
    if(uSet!=NULL) {
        uSet->evaluate(uDesired,0,tiReal);
    } else {
        qSet->evaluate(uDesired,1,tiReal);
    }
    Array<double> qCorrection(0.0,nq),uCorrection(0.0,nu);
       const Vector& q = s.getQ();
       const Vector& u = s.getU();

    for(i=0;i<nq;i++) qCorrection[i] = q[i] - qDesired[i];
    for(i=0;i<nu;i++) uCorrection[i] = u[i] - uDesired[i];

    _predictor->getCMCActSubsys()->setCoordinateCorrections(&qCorrection[0]);
    _predictor->getCMCActSubsys()->setSpeedCorrections(&uCorrection[0]);

    if( _verbose ) {
        cout << "\n=============================" << endl;
        cout << "\nCMC:computeControls"  << endl;
        cout << "\nq's = " << s.getQ() << endl;
        cout << "\nu's = " << s.getU() << endl;
        cout << "\nz's = " << s.getZ() << endl;
        cout<<"\nqDesired:"<<qDesired << endl;
        cout<<"\nuDesired:"<<uDesired << endl;
        cout<<"\nQCorrections:"<<qCorrection << endl;
        cout<<"\nUCorrections:"<<uCorrection << endl;
    }

    // realize to Velocity because some tasks (eg. CMC_Point) need to be
    // at velocity to compute errors
    _model->getMultibodySystem().realize(s, Stage::Velocity );

    // ERRORS
    _taskSet->computeErrors(s, tiReal);
    _taskSet->recordErrorsAsLastErrors();
    Array<double> &pErr = _taskSet->getPositionErrors();
    Array<double> &vErr = _taskSet->getVelocityErrors();
    if(_verbose) cout<<"\nErrors at time "<<s.getTime()<<":"<<endl;
    int e=0;
    for(i=0;i<_taskSet->getSize();i++) {
        
        TrackingTask& task = _taskSet->get(i);

        if(_verbose) {
            for(j=0;j<task.getNumTaskFunctions();j++) {
                cout<<task.getName()<<":  ";
                cout<<"pErr="<<pErr[e]<<" vErr="<<vErr[e]<<endl;
                e++;
            }
        }
    }

    double *err = new double[pErr.getSize()];
    for(i=0;i<pErr.getSize();i++) err[i] = pErr[i];
    _pErrStore->append(tiReal,pErr.getSize(),err);
    for(i=0;i<vErr.getSize();i++) err[i] = vErr[i];
    _vErrStore->append(tiReal,vErr.getSize(),err);

    
    // COMPUTE DESIRED ACCELERATIONS
    _taskSet->computeDesiredAccelerations(s, tiReal,tfReal);

    // Set the weight of the stress term in the optimization target based on this sigmoid-function-blending
    // Note that if no task limits are set then by default the weight will be 1.
    // TODO: clean this up -- currently using dynamic_casts to make sure we're not using fast target, etc.
    if(dynamic_cast<ActuatorForceTarget*>(_target)) {
        double relativeTau = 0.1;
        ActuatorForceTarget *realTarget = dynamic_cast<ActuatorForceTarget*>(_target);
//.........这里部分代码省略.........

/**
 * Compute the initial states for a simulation.
 *
 * The caller should send in an initial guess.  The Qs and Us are set
 * based on the desired trajectories.  The actuator states are set by
 * solving for a desired set of actuator forces, and then letting the states
 * come to equilibrium for those forces.
 *
 * @param rTI Initial time in normalized time.  Note this is changed to
 * the time corresponding to the new initial states on return.
 * @param s Initial states.
 */
void CMC::
computeInitialStates(SimTK::State& s, double &rTI)
{
    
    int i,j;

    int N = _predictor->getNX();
    SimTK::State initialState = s;
    Array<double> xmin(0.01,N),forces(0.0,N);

    double tiReal = rTI;
    if( _verbose ) {
        cout<<"\n\n=============================================\n";
        cout<<"enter CMC.computeInitialStates: ti="<< rTI << "  q's=" << s.getQ() <<endl;
        cout<<"\nenter CMC.computeInitialStates: ti="<< rTI << "  u's=" << s.getU() <<endl;
        cout<<"\nenter CMC.computeInitialStates: ti="<< rTI << "  z's=" << s.getZ() <<endl;
        cout<<"=============================================\n";
    }


    // TURN ANALYSES OFF
    _model->updAnalysisSet().setOn(false);

    // CONSTRUCT CONTROL SET
    ControlSet xiSet;

    for(i=0;i< getNumControls();i++) {
        ControlConstant *x = new ControlConstant();
        x->setName(_controlSet.get(i).getName());
        x->setIsModelControl(true);
        // This is not a very good way to set the bounds on the controls because ConrtolConstant only supports constant
        // min/max bounds but we may have time-dependent min/max curves specified in the controls constraints file
        //
        Control& xPredictor = _controlSet.get(i);
        x->setDefaultParameterMin(xPredictor.getDefaultParameterMin());
        x->setDefaultParameterMax(xPredictor.getDefaultParameterMax());
        double xmin = xPredictor.getControlValueMin(tiReal);
        if(!SimTK::isNaN(xmin)) x->setControlValueMin(tiReal,xmin);
        double xmax = xPredictor.getControlValueMax(tiReal);
        if(!SimTK::isNaN(xmax)) x->setControlValueMax(tiReal,xmax);
        xiSet.adoptAndAppend(x);
    }

    // ACTUATOR EQUILIBRIUM
    // 1
    //
    // perform integration but reset the coords and speeds so only actuator
    // states at changed

    obtainActuatorEquilibrium(s,tiReal,0.200,xmin,true);
    if( _verbose ) {
        cout<<"\n\n=============================================\n";
        cout<<"#1 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  q's=" << s.getQ() <<endl;
        cout<<"\n#1 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  u's=" << s.getU() <<endl;
        cout<<"\n#1 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  z's=" << s.getZ() <<endl;
        cout<<"=============================================\n";
    }
    restoreConfiguration( s, initialState ); // set internal coord,speeds to initial vals. 

    // 2
    obtainActuatorEquilibrium(s,tiReal,0.200,xmin,true);
    if( _verbose ) {
        cout<<"\n\n=============================================\n";
        cout<<"#2 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  q's=" << s.getQ() <<endl;
        cout<<"\n#2 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  u's=" << s.getU() <<endl;
        cout<<"\n#2 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  z's=" << s.getZ() <<endl;
        cout<<"=============================================\n";
    }
    restoreConfiguration( s, initialState );

    // CHANGE THE TARGET DT ON THE CONTROLLER TEMPORARILY
    double oldTargetDT = getTargetDT();
    double newTargetDT = 0.030;
    setTargetDT(newTargetDT);

    // REPEATEDLY CONTROL OVER THE FIRST TIME STEP
    Array<double> xi(0.0, getNumControls());
    for(i=0;i<2;i++) {

        // CLEAR ANY PREVIOUS CONTROL NODES
        for(j=0;j<_controlSet.getSize();j++) {
            ControlLinear& control = (ControlLinear&)_controlSet.get(j);
            control.clearControlNodes();
        }

        // COMPUTE CONTROLS

        s.updTime() = rTI;
//.........这里部分代码省略.........

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