C++ ScalarActuator::getStress方法代码示例

void StaticOptimizationTarget::
computeActuatorAreas(const SimTK::State& s )
{
    // COMPUTE ACTUATOR AREAS
    ForceSet& forceSet = _model->updForceSet();
    for(int i=0, j=0;i<forceSet.getSize();i++) {
        ScalarActuator *act = dynamic_cast<ScalarActuator*>(&forceSet.get(i));
        if( act ) {
             act->setActuation(s, 1.0);
             _recipAreaSquared[j] = act->getStress(s);
             _recipAreaSquared[j] *= _recipAreaSquared[j];
             j++;
        }
    }
}

void ActuatorForceTarget::
computePerformanceVectors(SimTK::State& s, const Vector &aF, Vector &rAccelPerformanceVector, Vector &rForcePerformanceVector)
{
    const Set<Actuator> &fSet = _controller->getActuatorSet();

    for(int i=0;i<fSet.getSize();i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        act->setOverrideActuation(s, aF[i]);
        act->overrideActuation(s, true);
    }

    _controller->getModel().getMultibodySystem().realize(s, SimTK::Stage::Acceleration );

    CMC_TaskSet& taskSet = _controller->updTaskSet();
    taskSet.computeAccelerations(s);
    Array<double> &w = taskSet.getWeights();
    Array<double> &aDes = taskSet.getDesiredAccelerations();
    Array<double> &a = taskSet.getAccelerations();

    // PERFORMANCE
    double sqrtStressTermWeight = sqrt(_stressTermWeight);
    for(int i=0;i<fSet.getSize();i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        rForcePerformanceVector[i] = sqrtStressTermWeight * act->getStress(s);
     }

    int nacc = aDes.getSize();
    for(int i=0;i<nacc;i++) rAccelPerformanceVector[i] = sqrt(w[i]) * (a[i] - aDes[i]);

    // reset the actuator control
    for(int i=0;i<fSet.getSize();i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        act->overrideActuation(s, false);
    }


}

/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 * This method should be overridden in the child class.  It is
 * included here so that the child class will not have to implement it if it
 * is not necessary.
 *
 * @param s state of system
 *
 * @return -1 on error, 0 otherwise.
 */
int InverseDynamics::begin(const SimTK::State& s )
{
    if(!proceed()) return(0);

    SimTK::Matrix massMatrix;
    _model->getMatterSubsystem().calcM(s, massMatrix);

    //massMatrix.dump("mass matrix is:");
    // Check that m is full rank
    try {
        SimTK::FactorLU lu(massMatrix);
    } catch (const SimTK::Exception::Base&) {
        throw Exception("InverseDynamics: ERROR- mass matrix is singular  ",__FILE__,__LINE__);
    }
    // enable the line below when simmath is fixed
    //bool singularMassMatrix = lu.isSingular();

    //cout << "Mass matrix is " << (singularMassMatrix?"singular":"Non-singular");

    // Make a working copy of the model
    delete _modelWorkingCopy;
    _modelWorkingCopy = _model->clone();
    _modelWorkingCopy->updAnalysisSet().setSize(0);

    // Replace model force set with only generalized forces
    if(_model) {
        SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->initSystem();
        // Update the _forceSet we'll be computing inverse dynamics for
        if(_ownsForceSet) delete _forceSet;
        if(_useModelForceSet) {
            // Set pointer to model's internal force set
            _forceSet = &_modelWorkingCopy->updForceSet();
            _numCoordinateActuators = _modelWorkingCopy->getActuators().getSize();
        } else {
            ForceSet& as = _modelWorkingCopy->updForceSet();
            // Keep a copy of forces that are not muscles to restore them back.
            ForceSet* saveForces = as.clone();
            // Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom
            _forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false);
            _numCoordinateActuators = _forceSet->getSize();
            // Copy whatever forces that are not muscles back into the model
            
            for(int i=0; i<saveForces->getSize(); i++){
                // const Force& f=saveForces->get(i);
                if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL)
                    as.append(saveForces->get(i).clone());
            }
        }
        _modelWorkingCopy->setAllControllersEnabled(false);
        _ownsForceSet = false;

        SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();

        // Gather indices into speed set corresponding to the unconstrained degrees of freedom (for which we will set acceleration constraints)
        _accelerationIndices.setSize(0);
        auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();
        for(size_t i=0u; i<coordinates.size(); ++i) {
            const Coordinate& coord = *coordinates[i];
            if(!coord.isConstrained(sWorkingCopy)) {
                _accelerationIndices.append(static_cast<int>(i));
            }
        }

        _dydt.setSize(_modelWorkingCopy->getNumCoordinates() + _modelWorkingCopy->getNumSpeeds());

        int nf = _numCoordinateActuators;
        int nacc = _accelerationIndices.getSize();

        if(nf < nacc) 
            throw(Exception("InverseDynamics: ERROR- over-constrained system -- need at least as many forces as there are degrees of freedom.\n"));

        // Realize to velocity in case there are any velocity dependent forces
        _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);

        _constraintMatrix.resize(nacc,nf);
        _constraintVector.resize(nacc);

        _performanceMatrix.resize(nf,nf);
        _performanceMatrix = 0;
        for(int i=0,j=0; i<nf; i++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
            if( act ) {
                act->setActuation(sWorkingCopy, 1);
                _performanceMatrix(j,j) = act->getStress(sWorkingCopy);
                j++;
             }
        }

//.........这里部分代码省略.........