C++ ScalarActuator类代码示例

//==============================================================================
// 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;
}

/**
 * Compute all constraints given x.
 */
void ActuatorForceTargetFast::
computeConstraintVector(SimTK::State& s, const Vector &x,Vector &c) const
{
    CMC_TaskSet&  taskSet = _controller->updTaskSet();
    const Set<Actuator>& fSet = _controller->getActuatorSet();

    int nf = fSet.getSize();

    // Now override the actuator forces with computed active force
    // (from static optimization) but also include the passive force
    // contribution of muscles when applying forces to the model
    for(int i=0;i<nf;i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        act->overrideActuation(s, true);
        act->setOverrideActuation(s, x[i]);
    }
    _controller->getModel().getMultibodySystem().realize(s, SimTK::Stage::Acceleration );

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

    // CONSTRAINTS
    for(int i=0; i<getNumConstraints(); i++)
        c[i]=w[i]*(aDes[i]-a[i]);

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

    _controller->getModel().getMultibodySystem().realizeModel(s);
}

//=============================================================================
// ACCELERATION
//=============================================================================
//
void StaticOptimizationTarget::
computeAcceleration(SimTK::State& s, const SimTK::Vector &parameters,SimTK::Vector &rAccel) const
{
    double time = s.getTime();
    

    const ForceSet& fs = _model->getForceSet();
    for(int i=0,j=0;i<fs.getSize();i++)  {
        ScalarActuator *act = dynamic_cast<ScalarActuator*>(&fs.get(i));
         if( act ) {
             act->setOverrideActuation(s, parameters[j] * _optimalForce[j]);
             j++;
         }
    }

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

    SimTK::Vector udot = _model->getMatterSubsystem().getUDot(s);

    for(int i=0; i<_accelerationIndices.getSize(); i++) 
        rAccel[i] = udot[_accelerationIndices[i]];

    //QueryPerformanceCounter(&stop);
    //double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart;
    //std::cout << "computeAcceleration time = " << (duration*1.0e3) << " milliseconds" << std::endl;

    // 1.45 ms
}

/**
 * Get the names of the states of the actuators.
 *
 * @param rNames Array of names.
 */
void ForceSet::
getStateVariableNames(OpenSim::Array<std::string> &rNames) const
{
    for(int i=0;i<getSize();i++) {
        ScalarActuator *act = dynamic_cast<ScalarActuator*>(&get(i));
       
        if(act) {
            rNames.append(act->getStateVariableNames());
        }
    }
}

/**
 * Get an optimal force.
 */
void StaticOptimizationTarget::
getActuation(SimTK::State& s, const SimTK::Vector &parameters, SimTK::Vector &forces)
{
    //return(_optimalForce[aIndex]);
    const ForceSet& fs = _model->getForceSet();
    SimTK::Vector tempAccel(getNumConstraints());
    computeAcceleration(s, parameters, tempAccel);
    for(int i=0,j=0;i<fs.getSize();i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fs.get(i));
        if( act )forces(j++) = act->getActuation(s);
    }
}

//==============================================================================
// 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;
}

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++;
        }
    }
}

/**
 * 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++;
    }
}

//=============================================================================
// ANALYSIS
//=============================================================================
//
void InverseDynamics::
computeAcceleration(SimTK::State& s, double *aF,double *rAccel) const
{
    for(int i=0,j=0; i<_forceSet->getSize(); i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
        if( act ) {
            act->setOverrideActuation(s, aF[j++]);
        }
    }

    // NEED TO APPLY OTHER FORCES (e.g. Prescribed) FROM ORIGINAL MODEL 

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

    SimTK::Vector udot = _modelWorkingCopy->getMatterSubsystem().getUDot(s);

    for(int i=0; i<_accelerationIndices.getSize(); i++) 
        rAccel[i] = udot[_accelerationIndices[i]];

}

/**
* Compute the Force.
*/
SimTK::Vector ActuatorForceProbe::computeProbeInputs(const State& s) const
{
	int nA = getActuatorNames().size();
	SimTK::Vector TotalF(getNumProbeInputs());
	TotalF = 0;

	// Loop through each actuator in the list of actuator_names.
	for (int i = 0; i<nA; ++i)
	{
		// Get the Actuator force.
		ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_model->getActuators()[_actuatorIndex[i]]);
		const double Ftmp = act->getForce(s);

		// Append to output vector.
		if (getSumForcesTogether())
			TotalF(0) += std::pow(Ftmp, getExponent());
		else
			TotalF(i) = std::pow(Ftmp, getExponent());
	}
	return TotalF;
}

/**
 * Evaluate the vector function.
 *
 * @param s SimTK::State.
 * @param aF Array of actuator force differences.
 */
void VectorFunctionForActuators::
evaluate( const SimTK::State& s, double *aX, double *rF) 
{
    int i;
    int N = getNX();

    CMC& controller=  dynamic_cast<CMC&>(_model->updControllerSet().get("CMC" ));
    controller.updControlSet().setControlValues(_tf, aX);

    // create a Manager that will integrate just the actuator subsystem and use only the 
    // CMC controller

    Manager manager(*_model, *_integrator);
    manager.setInitialTime(_ti);
    manager.setFinalTime(_tf);
    manager.setSystem( _CMCActuatorSystem );
    // tell the mangager to not call the analayses or write to storage 
    // while the CMCSubsystem is being integrated.
    manager.setPerformAnalyses(false); 
    manager.setWriteToStorage(false); 
    SimTK::State& actSysState = _CMCActuatorSystem->updDefaultState();
    getCMCActSubsys()->updZ(actSysState) = _model->getMultibodySystem()
                                            .getDefaultSubsystem().getZ(s);

    actSysState.setTime(_ti);

    // Integration
    manager.integrate(actSysState, 0.000001);

    const Set<Actuator>& forceSet = controller.getActuatorSet();
    // Vector function values
    int j = 0;
    for(i=0;i<N;i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&forceSet[i]);
        rF[j] = act->getActuation(getCMCActSubsys()->getCompleteState()) - _f[j];
        j++;
    }


}

/**
* Record the actuation quantities.
*/
int Actuation::
record(const SimTK::State& s)
{
    if (_model == NULL) return(-1);

    // MAKE SURE ALL ACTUATION QUANTITIES ARE VALID
    _model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics);

    // TIME NORMALIZATION
    double tReal = s.getTime();

    // FORCE
    const Set<Actuator>& fSet = _model->getActuators();
    for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        if (!fSet.get(i).get_isDisabled())
            _fsp[iact++] = act->getActuation(s);
    }
    _forceStore->append(tReal, _na, _fsp);

    // SPEED
    for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        if (!fSet.get(i).get_isDisabled())
            _fsp[iact++] = act->getSpeed(s);
    }
    _speedStore->append(tReal, _na, _fsp);

    // POWER
    for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
        if (!fSet.get(i).get_isDisabled())
            _fsp[iact++] = fSet.get(i).getPower(s);
    }
    _powerStore->append(tReal, _na, _fsp);


    return(0);
}

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 is meant to be called at the beginning of an integration 
 *
 * @param s Current state .
 *
 * @return -1 on error, 0 otherwise.
 */
int StaticOptimization::
begin(SimTK::State& s )
{
    if(!proceed()) return(0);

    // Make a working copy of the model
    delete _modelWorkingCopy;
    _modelWorkingCopy = _model->clone();
    _modelWorkingCopy->initSystem();

    // Replace model force set with only generalized forces
    if(_model) {
        SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->updWorkingState();
        // 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();
            _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
//.........这里部分代码省略.........

/**
 * 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;
//.........这里部分代码省略.........