Exemplo n.º 1
0
bool State::isConsistent(const SimTK::State& otherState) const {

    if (getNumSubsystems() != otherState.getNumSubsystems())
        return false;

    // State variables.
    if (getNQ() != otherState.getNQ())
        return false;
    if (getNU() != otherState.getNU())
        return false;
    if (getNZ() != otherState.getNZ())
        return false;

    // Constraints.
    if (getNQErr() != otherState.getNQErr())
        return false;
    if (getNUErr() != otherState.getNUErr())
        return false;
    if (getNUDotErr() != otherState.getNUDotErr())
        return false;
    // NMultipliers should be the same as NUDotErr, but we leave this check
    // here in case they diverge in the future.
    if (getNMultipliers() != otherState.getNMultipliers())
        return false;

    // Events.
    if (getNEventTriggers() != otherState.getNEventTriggers())
        return false;

    // Per-subsystem quantities.
    // TODO we could get rid of the total-over-subsystems checks above, but
    // those checks would let us exit earlier.
    for (SimTK::SubsystemIndex isub(0); isub < getNumSubsystems();
            ++isub) {
        if (getNQ(isub) != otherState.getNQ(isub))
            return false;
        if (getNU(isub) != otherState.getNU(isub))
            return false;
        if (getNZ(isub) != otherState.getNZ(isub))
            return false;
        if (getNQErr(isub) != otherState.getNQErr(isub))
            return false;
        if (getNUErr(isub) != otherState.getNUErr(isub))
            return false;
        if (getNUDotErr(isub) != otherState.getNUDotErr(isub))
            return false;
        // NMultipliers should be the same as NUDotErr, but we leave this check
        // here in case they diverge in the future.
        if (getNMultipliers(isub) != otherState.getNMultipliers(isub))
            return false;

        for(SimTK::Stage stage = SimTK::Stage::LowestValid;
                stage <= SimTK::Stage::HighestRuntime; ++stage) {
            if (getNEventTriggersByStage(isub, stage) !=
                    otherState.getNEventTriggersByStage(isub, stage))
                return false;
        }
    }
    return true;
}
Exemplo n.º 2
0
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);
}