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

void compareSimulations(SimTK::MultibodySystem &system, SimTK::State &state, Model *osimModel, SimTK::State &osim_state, string errorMessagePrefix = "")
{
    using namespace SimTK;

    // Set the initial states for both Simbody system and OpenSim model
    Vector& qi = state.updQ();
    Vector& ui = state.updU();
    int nq_sb = initTestStates(qi, ui);
    int nq = osim_state.getNQ();

    // Push down to OpenSim "state"
        if(nq == 2*nq_sb){ //more coordinates because OpenSim model is constrained
            osim_state.updY()[0] = state.getY()[0];
            osim_state.updY()[1] = state.getY()[1];
            osim_state.updY()[nq] = state.getY()[nq_sb];
            osim_state.updY()[nq+1] = state.getY()[nq_sb+1];
        }
        else    
            osim_state.updY() = state.getY();
    

    //==========================================================================================================
    // Integrate Simbody system
    integrateSimbodySystem(system, state);

    // Simbody model final states
    qi = state.updQ();
    ui = state.updU();

    qi.dump("\nSimbody Final q's:");
    ui.dump("\nSimbody Final u's:");

    //==========================================================================================================
    // Integrate OpenSim model
    integrateOpenSimModel(osimModel, osim_state);

    // Get the state at the end of the integration from OpenSim.
    Vector& qf = osim_state.updQ();
    Vector& uf = osim_state.updU();
    cout<<"\nOpenSim Final q's:\n "<<qf<<endl;
    cout<<"\nOpenSim Final u's:\n "<<uf<<endl;

    //==========================================================================================================
    // Compare Simulation Results
    compareSimulationStates(qi, ui, qf, uf, errorMessagePrefix);
}

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

SimTK::Vector MomentArmSolver::computeCouplingVector(SimTK::State &state, 
        const Coordinate &coordinate) const
{
    // make sure copy of the state is realized to at least instance
    getModel().getMultibodySystem().realize(state, SimTK::Stage::Instance);

    // unlock the coordinate if it is locked
    coordinate.setLocked(state, false);

    // Calculate coupling matrix C to determine the influence of other coordinates 
    // (mobilities) on the coordinate of interest due to constraints
    state.updU() = 0;
    // Light-up speed of coordinate of interest and see how other coordinates
    // affected by constraints respond
    coordinate.setSpeedValue(state, 1);
    getModel().getMultibodySystem().realize(state, SimTK::Stage::Velocity);

    // Satisfy all the velocity constraints.
    getModel().getMultibodySystem().projectU(state, 1e-10);
    
    // Now calculate C. by checking how speeds of other coordinates change
    // normalized by how much the speed of the coordinate of interest changed 
    return state.getU() / coordinate.getSpeedValue(state);
}