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

/**
 * Integrate the equations of motion for the specified model.
 *
 * This method starts the integration at the initial default states of
 * the model.
 */
bool Manager::
integrate( SimTK::State& s, double dtFirst )
{
    
    int step = 0;

    s.setTime( _ti );

    // INTEGRATE
    return(doIntegration(s, step, dtFirst));

}

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