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

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

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


}

/**
 * Record the inverse dynamics forces.
 */
int InverseDynamics::
record(const SimTK::State& s)
{
    if(!_modelWorkingCopy) return -1;

//cout << "\nInverse Dynamics record() : \n" << endl;
    // Set model Q's and U's
    SimTK::State sWorkingCopy = _modelWorkingCopy->getWorkingState();

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

    // Having updated the model, at least re-realize Model stage!
    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Model);

    sWorkingCopy.setTime(s.getTime());
    sWorkingCopy.setQ(s.getQ());
    sWorkingCopy.setU(s.getU());


    // Having updated the states, at least realize to velocity!
    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);

    int nf = _numCoordinateActuators;
    int nacc = _accelerationIndices.getSize();
    // int nq = _modelWorkingCopy->getNumCoordinates();

//cout << "\nQ= " << s.getQ() << endl;
//cout << "\nU= " << s.getU() << endl;
    // Build linear constraint matrix and constant constraint vector
    SimTK::Vector f(nf), c(nacc);
    f = 0;
    computeAcceleration(sWorkingCopy, &f[0], &_constraintVector[0]);

    for(int j=0; j<nf; j++) {
        f[j] = 1;
        computeAcceleration(sWorkingCopy, &f[0], &c[0]);
        for(int i=0; i<nacc; i++) _constraintMatrix(i,j) = (c[i] - _constraintVector[i]);
        f[j] = 0;
    }

    auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();

    for(int i=0; i<nacc; i++) {
        const Coordinate& coord = *coordinates[_accelerationIndices[i]];
        int ind = _statesStore->getStateIndex(coord.getSpeedName(), 0);
        if (ind < 0){
            // get the full coordinate speed state variable path name
            string fullname = coord.getStateVariableNames()[1];
            ind = _statesStore->getStateIndex(fullname, 0);
            if (ind < 0){
                string msg = "InverseDynamics::record(): \n";
                msg += "target motion for coordinate '";
                msg += coord.getName() + "' not found.";
                throw Exception(msg);
            }
        }
        Function& targetFunc = _statesSplineSet.get(ind);
        std::vector<int> firstDerivComponents(1);
        firstDerivComponents[0]=0;
        double targetAcceleration = targetFunc.calcDerivative(firstDerivComponents, SimTK::Vector(1, sWorkingCopy.getTime()));
//cout <<  coord.getName() << " t=" << sWorkingCopy.getTime() << "  acc=" << targetAcceleration << " index=" << _accelerationIndices[i] << endl; 
        _constraintVector[i] = targetAcceleration - _constraintVector[i];
    }
    //cout << "NEW Constraint Vector Adjusted = " << endl;
    //_constraintVector.dump(&t);

    // LAPACK SOLVER
    // NOTE: It destroys the matrices/vectors we pass to it, so we need to pass it copies of performanceMatrix and performanceVector (don't bother making
    // copies of _constraintMatrix/Vector since those are reinitialized each time anyway)
    int info;
    SimTK::Matrix performanceMatrixCopy = _performanceMatrix;
    SimTK::Vector performanceVectorCopy = _performanceVector;
//cout << "performanceMatrixCopy : " << performanceMatrixCopy << endl;
//cout << "performanceVectorCopy : " << performanceVectorCopy << endl;
//cout << "_constraintMatrix : " << _constraintMatrix << endl;
//cout << "_constraintVector : " << _constraintVector << endl;
//cout << "nf=" << nf << "  nacc=" << nacc << endl;
    dgglse_(nf, nf, nacc, &performanceMatrixCopy(0,0), nf, &_constraintMatrix(0,0), nacc, &performanceVectorCopy[0], &_constraintVector[0], &f[0], &_lapackWork[0], _lapackWork.size(), info);

    // Record inverse dynamics forces
    _storage->append(sWorkingCopy.getTime(),nf,&f[0]);

//cout << "\n ** f : " << f << endl << endl;

    return 0;
}

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

//.........这里部分代码省略.........
                _constraintSet[i].calcConstraintForces(s_solver, constraintBodyForces, constraintMobilityForces);
        }
        constraintBodyForces.dump("Constraint Body Forces");
        constraintMobilityForces.dump("Constraint Mobility Forces");
        // ******************************* end ERROR CHECKING *******************************/
    
        for(int i=0; i<constraintOn.getSize(); i++) {
            _replacementConstraints[i].setDisabled(s_solver, !constraintOn[i]);
            // Make sure we stay at Dynamics so each constraint can evaluate its conditions
            _modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
        }

        // This should also push changes to defaults for unilateral conditions
        _modelCopy.setPropertiesFromState(s_solver);

    }
    else if(forceName == "gravity"){
        // Set gravity ON
        _modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), false);

        // zero velocity
        s_solver.setU(SimTK::Vector(nu,0.0));

        // disable other forces
        for(int f=0; f<_modelCopy.getForceSet().getSize(); f++){
            _modelCopy.updForceSet()[f].setDisabled(s_solver, true);
        }
    }
    else if(forceName == "velocity"){       
        // Set gravity off
        _modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), true);

        // non-zero velocity
        s_solver.updU() = s.getU();
            
        // zero actuator forces
        for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
            _modelCopy.updActuators().get(f).setDisabled(s_solver, true);
        }
        // Set the configuration (gen. coords and speeds) of the model.
        _modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Velocity);
    }
    else{ //The rest are actuators      
        // Set gravity OFF
        _modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), true);

        // zero actuator forces
        for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
            _modelCopy.updActuators().get(f).setDisabled(s_solver, true);
        }

        // zero velocity
        SimTK::Vector U(nu,0.0);
        s_solver.setU(U);
        s_solver.updZ() = s.getZ();
        // light up the one Force who's contribution we are looking for
        int ai = _modelCopy.getForceSet().getIndex(forceName);
        if(ai<0){
            cout << "Force '"<< forceName << "' not found in model '" <<
                _modelCopy.getName() << "'." << endl;
        }
        Force &force = _modelCopy.getForceSet().get(ai);
        force.setDisabled(s_solver, false);

        ScalarActuator *actuator = dynamic_cast<ScalarActuator*>(&force);
        if(actuator){
            if(computeActuatorPotentialOnly){
                actuator->overrideActuation(s_solver, true);
                actuator->setOverrideActuation(s_solver, 1.0);
            }
        }

        // Set the configuration (gen. coords and speeds) of the model.
        _modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Model);
        _modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Velocity);

    }// End of if to select contributor 

    // cout << "Constraint 0 is of "<< _constraintSet[0].getConcreteClassName() << " and should be " << constraintOn[0] << " and is actually " <<  (_constraintSet[0].isDisabled(s_solver) ? "off" : "on") << endl;
    // cout << "Constraint 1 is of "<< _constraintSet[1].getConcreteClassName() << " and should be " << constraintOn[1] << " and is actually " <<  (_constraintSet[1].isDisabled(s_solver) ? "off" : "on") << endl;

    // After setting the state of the model and applying forces
    // Compute the derivative of the multibody system (speeds and accelerations)
    _modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);

    // Sanity check that constraints hasn't totally changed the configuration of the model
    // double error = (s.getQ()-s_solver.getQ()).norm();

    // Report reaction forces for debugging
    /*
    SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize());
    SimTK::Vector mobilityForces(0);

    for(int i=0; i<constraintOn.getSize(); i++) {
        if(constraintOn[i])
            _constraintSet.get(i).calcConstraintForces(s_solver, constraintBodyForces, mobilityForces);
    }*/

    return s_solver.getUDot();
}