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