void compareSimulationStates(SimTK::Vector q_sb, SimTK::Vector u_sb,
SimTK::Vector q_osim, SimTK::Vector u_osim,
string errorMessagePrefix = "")
{
using namespace SimTK;
Vector q_err = q_sb;
Vector u_err = u_sb;
int nq = q_osim.size();
if(q_sb.size() > nq){ // we have an unused quaternion slot in Simbody
q_sb.dump("Simbody q's:");
q_osim.dump("OpenSim q's:");
//This is a hack knowing the free and ball joint tests have the
// quaternion q's first and that the q's are packed as qqqq or aaa*
// for a ball and qqqqxyz or aaaxyz* for a free joint
int quat_ind = ((nq > 6) ? 6 : 3);
int j = 0;
for(int i=0; i< q_sb.size(); i++){
if(i != quat_ind){
q_err[j] = q_sb[i] - q_osim[j];
j++;
}
}
}
else{
if(nq > q_sb.size()){ // OpenSim using constrains
q_err[0] = q_sb[0] - q_osim[0];
q_err[1] = q_sb[1] - q_osim[1];
u_err[0] = u_sb[0] - u_osim[0];
u_err[1] = u_sb[1] - u_osim[1];
}
else{
q_err = q_sb - q_osim;
u_err = u_sb - u_osim;
}
}
//cout<<"\nSimbody - OpenSim:"<<endl;
//q_err.dump("Diff q's:");
//u_err.dump("Diff u's:");
stringstream errorMessage1, errorMessage2;
errorMessage1 << "testConstraints compareSimulationStates failed q_err.norm = " << q_err.norm();
errorMessage2 << "testConstraints compareSimulationStates failed u_err.norm = " << u_err.norm();
ASSERT(q_err.norm() <= 10*integ_accuracy, __FILE__, __LINE__, errorMessagePrefix + errorMessage1.str());
ASSERT(u_err.norm() <= 20*integ_accuracy, __FILE__, __LINE__, errorMessagePrefix + errorMessage2.str());
}
