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

//==============================================================================
// CONSTRUCTION AND DESTRUCTION
//==============================================================================
bool ActuatorForceTargetFast::
prepareToOptimize(SimTK::State& s, double *x)
{
    // Keep around a "copy" of the state so we can use it in objective function 
    // in cases where we're tracking states
    _saveState = s;
#ifdef USE_LINEAR_CONSTRAINT_MATRIX
    int nf = _controller->getActuatorSet().getSize();
    int nc = getNumConstraints();

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

    Vector f(nf), c(nc);

    // Build linear constraint matrix and constant constraint vector
    f = 0;

    computeConstraintVector(s, f, _constraintVector);

    for(int j=0; j<nf; j++) {
        f[j] = 1;
        computeConstraintVector(s, f, c);
        _constraintMatrix(j) = (c - _constraintVector);
        f[j] = 0;
    }
#endif

    // use temporary copy of state because computeIsokineticForceAssumingInfinitelyStiffTendon
    // will change the muscle states. This is necessary ONLY in the case of deprecated muscles
    SimTK::State tempState = s;
    double activation = 1.0;
    getController()->getModel().getMultibodySystem().realize( tempState, SimTK::Stage::Dynamics );

    // COMPUTE MAX ISOMETRIC FORCE
    const Set<Actuator>& fSet = _controller->getActuatorSet();
    
    double fOpt = SimTK::NaN;

    getController()->getModel().getMultibodySystem().realize(tempState, SimTK::Stage::Dynamics );
    for(int i=0 ; i<fSet.getSize(); ++i) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        Muscle* mus = dynamic_cast<Muscle*>(act);
        if(mus==NULL) {
            fOpt = act->getOptimalForce();
        }
        else{   
            fOpt = mus->calcInextensibleTendonActiveFiberForce(tempState,
                                                              activation);
        }
        
        if( std::fabs(fOpt) < SimTK::TinyReal )
            fOpt = SimTK::TinyReal;

        _recipOptForceSquared[i] = 1.0 / (fOpt*fOpt);   
    }
    
    // return false to indicate that we still need to proceed with optimization (did not do a lapack direct solve)
    return false;
}

//==============================================================================
// CONSTRUCTION
//==============================================================================
bool StaticOptimizationTarget::
prepareToOptimize(SimTK::State& s, double *x)
{
    // COMPUTE MAX ISOMETRIC FORCE
    const ForceSet& fSet = _model->getForceSet();
    
    for(int i=0, j=0;i<fSet.getSize();i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet.get(i));
         if( act ) {
             double fOpt;
             Muscle *mus = dynamic_cast<Muscle*>(&fSet.get(i));
             if( mus ) {
                //ActivationFiberLengthMuscle *aflmus = dynamic_cast<ActivationFiberLengthMuscle*>(mus);
                if(mus && _useMusclePhysiology) {
                    _model->setAllControllersEnabled(true);
                    fOpt = mus->calcInextensibleTendonActiveFiberForce(s, 1.0);
                    _model->setAllControllersEnabled(false);
                } else {
                    fOpt = mus->getMaxIsometricForce();
                }
             } else {
                  fOpt = act->getOptimalForce();
             }
            _optimalForce[j++] = fOpt;
         }
    }

#ifdef USE_LINEAR_CONSTRAINT_MATRIX
    //cout<<"Computing linear constraint matrix..."<<endl;
    int np = getNumParameters();
    int nc = getNumConstraints();

    _constraintMatrix.resize(nc,np);
    _constraintVector.resize(nc);

    Vector pVector(np), cVector(nc);

    // Build linear constraint matrix and constant constraint vector
    pVector = 0;
    computeConstraintVector(s, pVector,_constraintVector);

    for(int p=0; p<np; p++) {
        pVector[p] = 1;
        computeConstraintVector(s, pVector, cVector);
        for(int c=0; c<nc; c++) _constraintMatrix(c,p) = (cVector[c] - _constraintVector[c]);
        pVector[p] = 0;
    }
#endif

    // return false to indicate that we still need to proceed with optimization
    return false;
}

/**
 * Constructor.
 *
 * @param aNX Number of controls.
 * @param aController Parent controller.
 */
ActuatorForceTargetFast::
ActuatorForceTargetFast(SimTK::State& s, int aNX,CMC *aController):
    OptimizationTarget(aNX), _controller(aController)
{
    // NUMBER OF CONTROLS
    if(getNumParameters()<=0) {
        throw(Exception("ActuatorForceTargetFast: ERROR- no controls.\n"));
    }

    // ALLOCATE STATE ARRAYS
    int ny = _controller->getModel().getNumStateVariables();
    int nq = _controller->getModel().getNumCoordinates();
    int nu = _controller->getModel().getNumSpeeds();
    int na = _controller->getActuatorSet().getSize();

    _y.setSize(ny);
    _dydt.setSize(ny);
    _dqdt.setSize(nq);
    _dudt.setSize(nu);
    _recipAreaSquared.setSize(na);
    _recipOptForceSquared.setSize(na);
    _recipAvgActForceRangeSquared.setSize(na);
    
    int nConstraints = _controller->getTaskSet().getNumActiveTaskFunctions();

    // NUMBERS OF CONSTRAINTS
    // There are only linear equality constraints.
    setNumEqualityConstraints(nConstraints);
    setNumLinearEqualityConstraints(nConstraints);

    // DERIVATIVE PERTURBATION SIZES;
    setDX(1.0e-6);

    // COMPUTE ACTUATOR AREAS
    Array<double> f(1.0,na);
    const Set<Actuator>& fSet = _controller->getActuatorSet();
    for(int i=0,j=0;i<fSet.getSize();i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        Muscle* musc = dynamic_cast<Muscle *>(act);
        if(musc)
            _recipAreaSquared[j] = f[j]/musc->getMaxIsometricForce();
        else
            _recipAreaSquared[j] = f[j]/act->getOptimalForce();
        
        _recipAreaSquared[j] *= _recipAreaSquared[j];
        j++;
    }
}