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

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

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

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

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

//=============================================================================
// HELPER
//=============================================================================
void AnalyzeTool::run(SimTK::State& s, Model &aModel, int iInitial, int iFinal, const Storage &aStatesStore, bool aSolveForEquilibrium)
{
    AnalysisSet& analysisSet = aModel.updAnalysisSet();

    for(int i=0;i<analysisSet.getSize();i++) {
        analysisSet.get(i).setStatesStore(aStatesStore);
    }

    // TODO: some sort of filtering or something to make derivatives smoother?
    GCVSplineSet statesSplineSet(5,&aStatesStore);

    // PERFORM THE ANALYSES
    double tPrev=0.0,t=0.0,dt=0.0;
    int ny = s.getNY();
    Array<double> dydt(0.0,ny);
    Array<double> yFromStorage(0.0,ny);

    const Array<string>& labels =  aStatesStore.getColumnLabels();
    int numOpenSimStates = labels.getSize()-1;

    SimTK::Vector stateData;
    stateData.resize(numOpenSimStates);

    for(int i=iInitial;i<=iFinal;i++) {
        tPrev = t;
        aStatesStore.getTime(i,s.updTime()); // time
        t = s.getTime();
        aModel.setAllControllersEnabled(true);

        aStatesStore.getData(i,numOpenSimStates,&stateData[0]); // states
        // Get data into local Vector and assign to State using common utility
        // to handle internal (non-OpenSim) states that may exist
        Array<std::string> stateNames = aStatesStore.getColumnLabels();
        for (int j=0; j<stateData.size(); ++j){
            // storage labels included time at index 0 so +1 to skip
            aModel.setStateVariableValue(s, stateNames[j+1], stateData[j]);
        }
       
        // Adjust configuration to match constraints and other goals
        aModel.assemble(s);

        // equilibrateMuscles before realization as it may affect forces
        if(aSolveForEquilibrium){
            try{// might not be able to equilibrate if model is in
                // a non-physical pose. For example, a pose where the 
                // muscle length is shorter than the tendon slack-length.
                // the muscle will throw an Exception in this case.
                aModel.equilibrateMuscles(s);
            }
            catch (const std::exception& e) {
                cout << "WARNING- AnalyzeTool::run() unable to equilibrate muscles ";
                cout << "at time = " << t <<"." << endl;
                cout << "Reason: " << e.what() << endl;
            }
        }
        // Make sure model is at least ready to provide kinematics
        aModel.getMultibodySystem().realize(s, SimTK::Stage::Velocity);

        if(i==iInitial) {
            analysisSet.begin(s);
        } else if(i==iFinal) {
            analysisSet.end(s);
        // Step
        } else {
            analysisSet.step(s,i);
        }
    }
}