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