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

/**
 * Record the results.
 */
int StaticOptimization::
record(const SimTK::State& s)
{
    if(!_modelWorkingCopy) return -1;

    // Set model to whatever defaults have been updated to from the last iteration
    SimTK::State& sWorkingCopy = _modelWorkingCopy->updWorkingState();
    sWorkingCopy.setTime(s.getTime());
    _modelWorkingCopy->initStateWithoutRecreatingSystem(sWorkingCopy); 

    // update Q's and U's
    sWorkingCopy.setQ(s.getQ());
    sWorkingCopy.setU(s.getU());

    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
    //_modelWorkingCopy->equilibrateMuscles(sWorkingCopy);

    const Set<Actuator>& fs = _modelWorkingCopy->getActuators();

    int na = fs.getSize();
    int nacc = _accelerationIndices.getSize();

    // IPOPT
    _numericalDerivativeStepSize = 0.0001;
    _optimizerAlgorithm = "ipopt";
    _printLevel = 0;
    //_optimizationConvergenceTolerance = 1e-004;
    //_maxIterations = 2000;

    // Optimization target
    _modelWorkingCopy->setAllControllersEnabled(false);
    StaticOptimizationTarget target(sWorkingCopy,_modelWorkingCopy,na,nacc,_useMusclePhysiology);
    target.setStatesStore(_statesStore);
    target.setStatesSplineSet(_statesSplineSet);
    target.setActivationExponent(_activationExponent);
    target.setDX(_numericalDerivativeStepSize);

    // Pick optimizer algorithm
    SimTK::OptimizerAlgorithm algorithm = SimTK::InteriorPoint;
    //SimTK::OptimizerAlgorithm algorithm = SimTK::CFSQP;

    // Optimizer
    SimTK::Optimizer *optimizer = new SimTK::Optimizer(target, algorithm);

    // Optimizer options
    //cout<<"\nSetting optimizer print level to "<<_printLevel<<".\n";
    optimizer->setDiagnosticsLevel(_printLevel);
    //cout<<"Setting optimizer convergence criterion to "<<_convergenceCriterion<<".\n";
    optimizer->setConvergenceTolerance(_convergenceCriterion);
    //cout<<"Setting optimizer maximum iterations to "<<_maximumIterations<<".\n";
    optimizer->setMaxIterations(_maximumIterations);
    optimizer->useNumericalGradient(false);
    optimizer->useNumericalJacobian(false);
    if(algorithm == SimTK::InteriorPoint) {
        // Some IPOPT-specific settings
        optimizer->setLimitedMemoryHistory(500); // works well for our small systems
        optimizer->setAdvancedBoolOption("warm_start",true);
        optimizer->setAdvancedRealOption("obj_scaling_factor",1);
        optimizer->setAdvancedRealOption("nlp_scaling_max_gradient",1);
    }

    // Parameter bounds
    SimTK::Vector lowerBounds(na), upperBounds(na);
    for(int i=0,j=0;i<fs.getSize();i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fs.get(i));
        if (act) {
            lowerBounds(j) = act->getMinControl();
            upperBounds(j) = act->getMaxControl();
            j++;
        }
    }
    
    target.setParameterLimits(lowerBounds, upperBounds);

    _parameters = 0; // Set initial guess to zeros

    // Static optimization
    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy,SimTK::Stage::Velocity);
    target.prepareToOptimize(sWorkingCopy, &_parameters[0]);

    //LARGE_INTEGER start;
    //LARGE_INTEGER stop;
    //LARGE_INTEGER frequency;

    //QueryPerformanceFrequency(&frequency);
    //QueryPerformanceCounter(&start);

    try {
        target.setCurrentState( &sWorkingCopy );
        optimizer->optimize(_parameters);
    }
    catch (const SimTK::Exception::Base& ex) {
        cout << ex.getMessage() << endl;
        cout << "OPTIMIZATION FAILED..." << endl;
        cout << endl;
        cout << "StaticOptimization.record:  WARN- The optimizer could not find a solution at time = " << s.getTime() << endl;
        cout << endl;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        if(_useModelForceSet) {
            // Set pointer to model's internal force set
            _forceSet = &_modelWorkingCopy->updForceSet();
            _ownsForceSet = false;
        } 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);
            _ownsForceSet = false;
            _modelWorkingCopy->setAllControllersEnabled(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());
            }
        }

        SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();
        // Set modeling options for Actuators to be overriden
        for(int i=0; i<_forceSet->getSize(); i++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
            if( act ) {
                act->overrideActuation(sWorkingCopy, true);
            }
        }

        sWorkingCopy.setTime(s.getTime());
        sWorkingCopy.setQ(s.getQ());
        sWorkingCopy.setU(s.getU());
        sWorkingCopy.setZ(s.getZ());
        _modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Velocity);
        _modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
        // Gather indices into speed set corresponding to the unconstrained degrees of freedom 
        // (for which we will set acceleration constraints)
        _accelerationIndices.setSize(0);
        const CoordinateSet& coordSet = _model->getCoordinateSet();
        for(int i=0; i<coordSet.getSize(); i++) {
            const Coordinate& coord = coordSet.get(i);
            if(!coord.isConstrained(sWorkingCopy)) {
                Array<int> inds = _statesStore->
                    getColumnIndicesForIdentifier(coord.getName()) ;
                _accelerationIndices.append(inds[0]);
            }
        }

        int na = _forceSet->getSize();
        int nacc = _accelerationIndices.getSize();

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

        _forceReporter.reset(new ForceReporter(_modelWorkingCopy));
        _forceReporter->begin(sWorkingCopy);
        _forceReporter->updForceStorage().reset();

        _parameters.resize(_modelWorkingCopy->getNumControls());
        _parameters = 0;
    }

    _statesSplineSet=GCVSplineSet(5,_statesStore);

    // DESCRIPTION AND LABELS
    constructDescription();
    constructColumnLabels();

    deleteStorage();
    allocateStorage();

    // RESET STORAGE
    _activationStorage->reset(s.getTime());
    _forceReporter->updForceStorage().reset(s.getTime());

    // RECORD
    int status = 0;
    if(_activationStorage->getSize()<=0) {
        status = record(s);
        const Set<Actuator>& fs = _modelWorkingCopy->getActuators();
        for(int k=0;k<fs.getSize();k++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator *>(&fs[k]);
            if (act){
                cout << "Bounds for " << act->getName() << ": "
                    << act->getMinControl() << " to "
                    << act->getMaxControl() << endl;
            }
            else{
                std::string msg = getConcreteClassName();
                msg += "::can only process scalar Actuator types.";
                throw Exception(msg);
            }
        }
    }

    return(status);
}

// for adding any components to the model
void CMC::extendAddToSystem( SimTK::MultibodySystem& system)  const
{
    Super::extendAddToSystem(system);

    // add event handler for updating controls for next window 
    CMC* mutableThis = const_cast<CMC *>(this);
    ComputeControlsEventHandler* computeControlsHandler = 
        new ComputeControlsEventHandler(mutableThis);

    system.updDefaultSubsystem().addEventHandler(computeControlsHandler );

    const Set<Actuator>& fSet = getActuatorSet();
    int nActs = fSet.getSize();

    mutableThis->_controlSetIndices.setSize(nActs);

    // Create the control set that will hold the controls computed by CMC
    mutableThis->_controlSet.setName(_model->getName());
    mutableThis->_controlSet.setSize(0);

    // Define the control set used to specify control bounds and to hold 
    // the computed control values from the CMC algorithm
    double xmin =0, xmax=0;

    std::string actName = "";
    
    for(int i=0; i < nActs; ++i ) {

        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        //Actuator& act = getActuatorSet().get(i);

        ControlLinear *control = new ControlLinear();
        control->setName(act->getName() + ".excitation" );

        xmin = act->getMinControl();
        if (xmin ==-SimTK::Infinity)
            xmin =-MAX_CONTROLS_FOR_RRA;
        
        xmax =  act->getMaxControl();
        if (xmax ==SimTK::Infinity)
            xmax =MAX_CONTROLS_FOR_RRA;

        Muscle *musc = dynamic_cast<Muscle *>(act);
        // if controlling muscles, CMC requires that the control be constant (i.e. piecewise constant or use steps)
        // since it uses this assumption to rootsolve for the required controls over the CMC time-window.
        if(musc){
            control->setUseSteps(true);
            if(xmin < MIN_CMC_CONTROL_VALUE){
                cout << "CMC::Warning: CMC cannot compute controls for muscles with muscle controls < " << MIN_CMC_CONTROL_VALUE <<".\n" <<
                    "The minimum control limit for muscle '" << musc->getName() << "' has been reset to " << MIN_CMC_CONTROL_VALUE <<"." <<endl;
                xmin = MIN_CMC_CONTROL_VALUE;
            }
            if(xmax < MAX_CMC_CONTROL_VALUE){
                cout << "CMC::Warning: CMC cannot compute controls for muscles with muscle controls > " << MAX_CMC_CONTROL_VALUE <<".\n" <<
                    "The maximum control limit for muscle '" << musc->getName() << "' has been reset to " << MAX_CMC_CONTROL_VALUE << "." << endl;
                xmax = MAX_CMC_CONTROL_VALUE;
            }
        }

        control->setDefaultParameterMin(xmin);
        control->setDefaultParameterMax(xmax);

        mutableThis->_controlSet.adoptAndAppend(control);
        mutableThis->_controlSetIndices.set(i, i);
    }

    mutableThis->setNumControls(_controlSet.getSize());
}