Exemple #1
0
/**
 * 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;
}
Exemple #2
0
//=============================================================================
// 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);
        }
    }
}