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