C++ State::getTime方法代码示例

/**
* 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 in
* Model::integBeginCallback() and has the same argument list.
*
* 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 state system State
* @param aStep Step number of the integration.
*
* @return -1 on error, 0 otherwise.
*/
int Actuation::
begin(SimTK::State& s)
{
    if (!proceed()) return(0);

    // NUMBER OF ACTUATORS
    int na = _model->getActuators().getSize();
    _na = na;
    // WORK ARRAY
    if (_fsp != NULL) delete[] _fsp;
    _fsp = new double[_na];

    // RESET STORAGE
    if (_forceStore == NULL)
        _forceStore = new Storage();
    if (_speedStore == NULL)
        _speedStore = new Storage();
    if (_powerStore == NULL)
        _powerStore = new Storage();

    // RESET STORAGE
    _forceStore->reset(s.getTime());
    _speedStore->reset(s.getTime());
    _powerStore->reset(s.getTime());

    // RECORD
    int status = 0;
    if (_forceStore->getSize() <= 0) {
        status = record(s);
    }

    return(status);
}

/**
 * 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 begining of an integration 
 *
 * This method should be overriden 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 current system state
 * @param aClientData General use pointer for sending in client data.
 *
 * @return -1 on error, 0 otherwise.
 */
int PointKinematics::
begin( SimTK::State& s)
{
	if(!proceed()) return(0);

	// RESET STORAGE
	_pStore->reset(s.getTime());
	_vStore->reset(s.getTime());
	_aStore->reset(s.getTime());

	int status = record(s);

	return(status);
}

/**
 * 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 begining of an integration 
 *
 * @param s current state of System
 *
 * @return -1 on error, 0 otherwise.
 */
int Kinematics::
begin( SimTK::State& s )
{
	if(!proceed()) return(0);

	double time = s.getTime();
	
	// RESET STORAGE
	_pStore->reset(time);
	_vStore->reset(time);
	if (_aStore) _aStore->reset(time);

	// RECORD
	int status = 0;
	if(_pStore->getSize()<=0) {
        if(_recordAccelerations && s.getSystemStage() < SimTK::Stage::Acceleration ) 
            _model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration);
		else
			_model->getMultibodySystem().realize(s, SimTK::Stage::Velocity);

		status = record(s);
	}

	return(status);
}

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

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

    StateVector nextRow = StateVector(s.getTime());

    // Model Forces
    auto forces = _model->getComponentList<Force>();

    for(auto& force : forces) {
        // If body force we need to record six values for torque+force
        // If muscle we record one scalar
        if (force.isDisabled(s)) continue;
        Array<double> values = force.getRecordValues(s);
        nextRow.getData().append(values);
    }

    if(_includeConstraintForces){
        // Model Constraints
        auto constraints = _model->getComponentList<Constraint>();
        for (auto& constraint : constraints) {
            if (constraint.isDisabled(s)) continue;
            Array<double> values = constraint.getRecordValues(s);
            nextRow.getData().append(values);
        }
    }
    _forceStore.append(nextRow);

    return(0);
}

/**
 * initialize storages and analyses 
 * 
 * @param s system state before integration
 */
void Manager::initialize(SimTK::State& s, double dt )
{
    // skip initializations for CMC's actuator system
    if( _writeToStorage && _performAnalyses ) { 

        double tReal = s.getTime();
    
        // STORE STARTING CONTROLS
        if (_model->isControlled()){
            _controllerSet->setModel(*_model);
            _controllerSet->storeControls(s, 0);
        }

        // STORE STARTING STATES
        if(hasStateStorage()) {
            // ONLY IF NO STATES WERE PREVIOUSLY STORED
            if(getStateStorage().getSize()==0) {
                SimTK::Vector stateValues = _model->getStateVariableValues(s);
                getStateStorage().store(0,tReal,stateValues.size(), &stateValues[0]);
            }
        }

        // ANALYSES 
        AnalysisSet& analysisSet = _model->updAnalysisSet();
        analysisSet.begin(s);
    }

    return;
}

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

    SimTK::Vec3 calcSomething(const SimTK::State& state) const {
        const_cast<Foo *>(this)->m_ctr++;
        m_mutableCtr++;

        double t = state.getTime();
        return SimTK::Vec3(t, t*t, sqrt(t));
    }

/**
 * Filter the controls.  This method was introduced as a means of attempting
 * to reduce the sizes of residuals.  Unfortunately, the approach was
 * generally unsuccessful because the desired accelerations were not
 * achieved.
 *
 * @param aControlSet Set of controls computed by CMC.
 * @param aDT Current time window.
 * @param rControls Array of filtered controls.
 */
void CMC::
FilterControls(const SimTK::State& s, const ControlSet &aControlSet,double aDT,
               OpenSim::Array<double> &rControls,bool aVerbosePrinting)
{
    if(aDT <= SimTK::Zero) {
        if(aVerbosePrinting) cout<<"\nCMC.filterControls: aDT is practically 0.0, skipping!\n\n";
        return;
    }

    if(aVerbosePrinting) cout<<"\n\nFiltering controls to limit curvature...\n";

    int i;
    int size = rControls.getSize();
    Array<double> x0(0.0,size),x1(0.0,size),x2(0.0,size);

    // SET TIMES
    double t0,t1/*,t2*/;
    // t2 = s.getTime();
    t1 = s.getTime() - aDT;
    t0 = t1 - aDT;

    // SET CONTROL VALUES
    x2 = rControls;
    aControlSet.getControlValues(t1,x1);
    aControlSet.getControlValues(t0,x0);

    // LOOP OVER CONTROLS
    double m1,m2;
    double curvature;
    double thresholdCurvature = 2.0 * 0.05 / (aDT * aDT);
    
    //double thresholdSlopeDiff = 0.2 / aDT;
    for(i=0;i<size;i++) {
        m2 = (x2[i]-x1[i]) / aDT;
        m1 = (x1[i]-x0[i]) / aDT;

                
        curvature = (m2 - m1) / aDT;
        curvature = fabs(curvature);

        if(curvature<=thresholdCurvature) continue;
    
//      diff = fabs(m2) - fabs(m1);
//      cout<<"thresholdSlopeDiff="<<thresholdSlopeDiff<<"  slopeDiff="<<diff<<endl;
//      if(diff>thresholdSlopeDiff) continue;
        
        

        // ALTER CONTROL VALUE
        rControls[i] = (3.0*x2[i] + 2.0*x1[i] + x0[i]) / 6.0;

        // PRINT
        if(aVerbosePrinting) cout<<aControlSet[i].getName()<<": old="<<x2[i]<<" new="<<rControls[i]<<endl;
    }

    if(aVerbosePrinting) cout<<endl<<endl;
}

/**
 * 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 
 *
 * 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 System state
 *
 * @return -1 on error, 0 otherwise.
 */
int BodyKinematics::
begin(SimTK::State& s )
{
    if(!proceed()) return(0);

    // RESET STORAGE
    _pStore->reset(s.getTime());
    _vStore->reset(s.getTime());
    _aStore->reset(s.getTime());

    // RECORD
    int status = 0;
    if(_pStore->getSize()<=0) {
        status = record(s);
    }

    return(status);
}

    void IKSolverParallel::pushState(const SimTK::State& s) {

        GeneralisedCoordinatesData currentData(nCoordinates_);
        std::vector<double> q(nCoordinates_);
        SimTK::Vector stateQ(s.getQ());
        for (unsigned i(0); i < nCoordinates_; ++i)
            q[i] = stateQ[i];
        currentData.setQ(q);
        outputGeneralisedCoordinatesQueue_.push({ s.getTime(), currentData });
    }

// compute the control value for an actuator
void PrescribedController::computeControls(const SimTK::State& s, SimTK::Vector& controls) const
{
    SimTK::Vector actControls(1, 0.0);
    SimTK::Vector time(1, s.getTime());

    for(int i=0; i<getActuatorSet().getSize(); i++){
        actControls[0] = get_ControlFunctions()[i].calcValue(time);
        getActuatorSet()[i].addInControls(actControls, controls);
    }  
}

void ControllerSet::storeControls( const SimTK::State& s, int step  )
{
    int size = _actuatorSet->getSize();
    
	if( size > 0 )
	{
		_controlStore->store( step, s.getTime(), getModel().getNumControls(), 
			                  &(getModel().getControls(s)[0]) );
    }
}

Array<bool> InducedAccelerationsSolver::
    applyContactConstraintAccordingToExternalForces(SimTK::State &s)
{
    Array<bool> constraintOn(false, _replacementConstraints.getSize());
    double t = s.getTime();

    for(int i=0; i<_forcesToReplace.getSize(); i++){
        ExternalForce* exf = dynamic_cast<ExternalForce*>(&_forcesToReplace[i]);
        SimTK::Vec3 point, force, gpoint;

        force = exf->getForceAtTime(t);
        
        // If the applied force is "significant" replace it with a constraint
        if (force.norm() > _forceThreshold){
            // get the point of contact from applied external force
            point = exf->getPointAtTime(t);
            // point should be expressed in the "applied to" body for consistency across all constraints
            if(exf->getPointExpressedInBodyName() != exf->getAppliedToBodyName()){
                int appliedToBodyIndex = getModel().getBodySet().getIndex(exf->getAppliedToBodyName());
                if(appliedToBodyIndex < 0){
                    cout << "External force appliedToBody " <<  exf->getAppliedToBodyName() << " not found." << endl;
                }

                int expressedInBodyIndex = getModel().getBodySet().getIndex(exf->getPointExpressedInBodyName());
                if(expressedInBodyIndex < 0){
                    cout << "External force expressedInBody " <<  exf->getPointExpressedInBodyName() << " not found." << endl;
                }

                const Body &appliedToBody = getModel().getBodySet().get(appliedToBodyIndex);
                const Body &expressedInBody = getModel().getBodySet().get(expressedInBodyIndex);

                getModel().getMultibodySystem().realize(s, SimTK::Stage::Velocity);
                getModel().getSimbodyEngine().transformPosition(s, expressedInBody, point, appliedToBody, point);
            }

            _replacementConstraints[i].setContactPointForInducedAccelerations(s, point);

            // turn on the constraint
            _replacementConstraints[i].setDisabled(s, false);
            // return the state of the constraint
            constraintOn[i] = true;

        }
        else{
            // turn off the constraint
            _replacementConstraints[i].setDisabled(s, true);
            // return the state of the constraint
            constraintOn[i] = false;
        }
    }

    return constraintOn;
}

void StaticOptimizationTarget::
printPerformance(const SimTK::State& s, double *parameters)
{
    double p;
    setCurrentState( &s );
    objectiveFunc(SimTK::Vector(getNumParameters(),parameters,true),true,p);
    SimTK::Vector constraints(getNumConstraints());
    constraintFunc(SimTK::Vector(getNumParameters(),parameters,true),true,constraints);
    cout << endl;
    cout << "time = " << s.getTime() <<" Performance =" << p << 
    " Constraint violation = " << sqrt(~constraints*constraints) << endl;
}

// Controller Interface. 
// compute the control value for all actuators this Controller is responsible for
void CMC::computeControls(const SimTK::State& s, SimTK::Vector& controls)  const
{
    SimTK_ASSERT( _controlSet.getSize() == getActuatorSet().getSize() , 
        "CMC::computeControls number of controls does not match number of actuators.");
    
    SimTK::Vector actControls(1, 0.0);
    for(int i=0; i<getActuatorSet().getSize(); i++){
        actControls[0] = _controlSet[_controlSetIndices[i]].getControlValue(s.getTime());
        getActuatorSet()[i].addInControls(actControls, controls);
    }

    // double *val = &controls[0];
}