/**
* Compute the Joint power.
*/
SimTK::Vector JointInternalPowerProbe::computeProbeInputs(const State& s) const
{
int nJ = getJointNames().size();
SimTK::Vector TotalP;
if (getSumPowersTogether()) {
TotalP.resize(1);
TotalP(0) = 0; // Initialize to zero
}
else
TotalP.resize(nJ);
// Loop through each joint in the list of joint_names
for (int i=0; i<nJ; ++i)
{
string jointName = getJointNames()[i];
int k = _model->getJointSet().getIndex(jointName);
// Get the "Joint" power from the Joint object
double jointPower = _model->getJointSet().get(k).calcPower(s);
// Append to output vector
if (getSumPowersTogether())
TotalP(0) += std::pow(jointPower, getExponent());
else
TotalP(i) = std::pow(jointPower, getExponent());
}
return TotalP;
}
//=============================================================================
// HELPER
//=============================================================================
void AnalyzeTool::run(SimTK::State& s, Model &aModel, int iInitial, int iFinal, const Storage &aStatesStore, bool aSolveForEquilibrium)
{
AnalysisSet& analysisSet = aModel.updAnalysisSet();
for(int i=0;i<analysisSet.getSize();i++) {
analysisSet.get(i).setStatesStore(aStatesStore);
}
// TODO: some sort of filtering or something to make derivatives smoother?
GCVSplineSet statesSplineSet(5,&aStatesStore);
// PERFORM THE ANALYSES
double tPrev=0.0,t=0.0,dt=0.0;
int ny = s.getNY();
Array<double> dydt(0.0,ny);
Array<double> yFromStorage(0.0,ny);
const Array<string>& labels = aStatesStore.getColumnLabels();
int numOpenSimStates = labels.getSize()-1;
SimTK::Vector stateData;
stateData.resize(numOpenSimStates);
for(int i=iInitial;i<=iFinal;i++) {
tPrev = t;
aStatesStore.getTime(i,s.updTime()); // time
t = s.getTime();
aModel.setAllControllersEnabled(true);
aStatesStore.getData(i,numOpenSimStates,&stateData[0]); // states
// Get data into local Vector and assign to State using common utility
// to handle internal (non-OpenSim) states that may exist
Array<std::string> stateNames = aStatesStore.getColumnLabels();
for (int j=0; j<stateData.size(); ++j){
// storage labels included time at index 0 so +1 to skip
aModel.setStateVariableValue(s, stateNames[j+1], stateData[j]);
}
// Adjust configuration to match constraints and other goals
aModel.assemble(s);
// equilibrateMuscles before realization as it may affect forces
if(aSolveForEquilibrium){
try{// might not be able to equilibrate if model is in
// a non-physical pose. For example, a pose where the
// muscle length is shorter than the tendon slack-length.
// the muscle will throw an Exception in this case.
aModel.equilibrateMuscles(s);
}
catch (const std::exception& e) {
cout << "WARNING- AnalyzeTool::run() unable to equilibrate muscles ";
cout << "at time = " << t <<"." << endl;
cout << "Reason: " << e.what() << endl;
}
}
// Make sure model is at least ready to provide kinematics
aModel.getMultibodySystem().realize(s, SimTK::Stage::Velocity);
if(i==iInitial) {
analysisSet.begin(s);
} else if(i==iFinal) {
analysisSet.end(s);
// Step
} else {
analysisSet.step(s,i);
}
}
}