C++ StateVector类代码示例

void
BCI2000OutputFormat::Write( ostream& os, const GenericSignal& inSignal, const StateVector& inStatevector )
{
  switch( mInputProperties.Type() )
  {
    case SignalType::int16:
    case SignalType::float32:
    case SignalType::int32:
      // Note that the order of Elements and Channels differs from the one in the
      // socket protocol.
      for( int j = 0; j < inSignal.Elements(); ++j )
      {
        for( int i = 0; i < inSignal.Channels(); ++i )
          inSignal.WriteValueBinary( os, i, j );
        os.write(
          reinterpret_cast<const char*>( inStatevector( min( j, inStatevector.Samples() - 1 ) ).Data() ),
          inStatevector.Length()
        );
      }
      break;

    default:
      bcierr << "Unsupported signal data type" << endl;
  }
}

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

	void TessaCreationApi::stopTrackingExtraVariable(int index) {
		StateVector* endState = currentFunction->getCurrentBasicBlock()->getEndState();
		int numberOfVariables = endState->getNumberOfLocals();
		int slotIndex = index + numberOfVariables;
		TessaAssert(index <= numberOfVariables)
		return endState->deleteLocal(slotIndex);
	}

// -------------------------------------------------------------------------------------------------
//  OperatorList::check(StateVector sv)
//
//  Use: Check the operator list for consitency
//  Input: StateVector sv - the spin configuration
//  Output: bool - true if ok, false if not
// -------------------------------------------------------------------------------------------------
bool OperatorList::check(StateVector sv)
{
    // Check that no diagonal or off-diagonal operator acts
    // on two parallel spins
    int N = sv.getSize();
    int L = getSize();

    for (int i = 0; i < L; i++) {
        int op = list->operators[i].op;
        int spin1 = list->operators[i].spin;
        int spin2 = spin1 + 1;
        if (spin2 > (N - 1))
            spin2 = 0;

        if ((op > 0) && (sv(spin1) == sv(spin2))) {
            cout << "Error: Operating on parallel spins!!" << endl;
            cout << "operator " << op << " on " << spin1 << "," << spin2;
            cout << " in " << sv << endl;
            cout << (*this) << endl;
            return false;
        }

        if (op == 2) {
            sv.flip(spin1);
            sv.flip(spin2);
        }
    }

    return true;
}

/**
 * Read sto file.
 *
 * @param aFilename name of sto file.
 */
void MarkerData::readStoFile(const string& aFileName)
{
    if (aFileName.empty())
        throw Exception("MarkerData.readStoFile: ERROR- Marker file name is empty",__FILE__,__LINE__);

    // If the file was written by STOFileAdapter, make the file readable by
    // Storage. Calls below have no effect otherwise.
    std::string tmpFileName{"tmp.sto"};
    bool versionChanged{revertToVersionNumber1(aFileName, tmpFileName)};
    if(versionChanged)
        addNumRowsNumColumns(tmpFileName, aFileName);
    std::remove(tmpFileName.c_str());

    Storage store(aFileName);

    // populate map between marker names and column numbers 
    std::map<int, std::string>  markerIndices;
    buildMarkerMap(store, markerIndices);

    if (markerIndices.size()==0){
        throw Exception("MarkerData.readStoFile: ERROR- No markers were identified. Markers should appear on consecutive columns as Marker1.x Marker1.y Marker1.z Marker2.x... etc.",__FILE__,__LINE__);
    }
    std::map<int, std::string>::iterator iter;

    for (iter = markerIndices.begin(); iter != markerIndices.end(); iter++) {
        SimTK::String markerNameWithSuffix = iter->second;
        size_t dotIndex =
            SimTK::String::toLower(markerNameWithSuffix).find_last_of(".x");
        SimTK::String candidateMarkerName = markerNameWithSuffix.substr(0, dotIndex-1);
        _markerNames.append(candidateMarkerName);
    }
    // use map to populate data for MarkerData header
    _numMarkers = (int) markerIndices.size();
    _numFrames = store.getSize();
    _firstFrameNumber = 1;
    _dataRate = 250;
    _cameraRate = 250;
    _originalDataRate = 250;
    _originalStartFrame = 1;
    _originalNumFrames = _numFrames;
    _fileName = aFileName;
    _units = Units(Units::Meters);

    double time;
    int sz = store.getSize();
    for (int i=0; i < sz; i++){
        StateVector* nextRow = store.getStateVector(i);
        time = nextRow->getTime();
        int frameNum = i+1;
        MarkerFrame *frame = new MarkerFrame(_numMarkers, frameNum, time, _units);
        const Array<double>& rowData = nextRow->getData();
        // Cycle through map and add Marker coordinates to the frame. Same order as header.
        for (iter = markerIndices.begin(); iter != markerIndices.end(); iter++) {
            int startIndex = iter->first; // startIndex includes time but data doesn't!
            frame->addMarker(SimTK::Vec3(rowData[startIndex-1], rowData[startIndex], rowData[startIndex+1]));
        }
        _frames.append(frame);
   }
}

bool StateVectorLimiter<StateType>::operator()(StateVector<StateType> &_stateVec) const
{
	if((_stateVec.size() != mUpper.size()) |(_stateVec.size() != mLower.size()))
	{
		std::cout << "incompatible dimensions. can't evaluate state limits\n";
		return false;
	}
}

/**
 * Copy constructor.
 */
StateVector::StateVector(const StateVector &aVector) :
	_data(0.0)
{
	// INITIAL VALUES
	setNull();

	// SET STATES
	setStates(aVector.getTime(),aVector.getSize(),&aVector.getData()[0]);
}

	/***
	 * Returns a shallow copy 
	 */
	StateVector* StateVector::clone() {
		StateVector *clone = new (gc) StateVector(currentReferences->size(), gc);

		for (int32_t i = 0; i < getNumberOfLocals(); i++) {
			TessaInstruction* currentReference = getLocal(i);
			clone->setLocal(i, currentReference);
		}

		return clone;
	}

// -------------------------------------------------------------------------------------------------
//  OperatorList::diagonalMove(StateVector& sv, double beta)
//
//  Use: Performs a diagonal update on the operator list
//  Input: StateVector& sv - the spin configuration
//         double beta - the inverse temperature
//  Output: none
// -------------------------------------------------------------------------------------------------
void OperatorList::diagonalUpdate(StateVector& sv, double beta)
{
    int N = sv.getSize();       // Number of spins
    int L = getSize();          // Operator list length

    for (int i = 0; i < L; i++) {
        int n = countDOD();   // Number of diagonal and off-diagonal operators
        int op = list->operators[i].op;
        if (op == 0) {
            // unit operator
            // Calculate probability to change operator
            double P = (double)(N*beta/(2*(L-n)));
            // Generate a random spin that this operator acts on
            int spin1 = (int)(ran()*N);
            // and its adjacent spin
            int spin2 = (spin1 + 1) % N;
            
            // Change operator to diagonal only if the spins are _not_ parallel...
            if (sv(spin1) != sv(spin2)) {
                // ...and with probability P
                double r = ran();

                if (r < P) {
                    //cout << "Change..." << endl;
                    list->operators[i].op = 1;
                    list->operators[i].spin = spin1;
                }
            }
        } else if (op == 1) { 
            // diagonal operator
            // Calculate probability to change to unit operator
            double P = (double)(2*(L-n+1)/(N*beta));
            double r = ran();
            
            // Get the spins this off-diagonal operator acts on
            int spin1 = list->operators[i].spin;
            int spin2 = (spin1 + 1) % N;
            
            if (r < P) {
                list->operators[i].op = 0;    
                // unit ops cannot act on spins
                list->operators[i].spin = -1;    
            }
        } else if (op == 2) { 
            // off-diagonal operator
            // Get the spins this off-diagonal operator acts on
            int spin1 = list->operators[i].spin;
            int spin2 = (spin1 + 1) % N;
            
            // Flip both spins
            sv.flip(spin1);
            sv.flip(spin2);                
        }
    }
} 

/** Get the angular velocity of the two-body rotating frame. It
  * is undefined whenever:
  *  - the state of either the primary or secondary object is undefined
  *  - the positions of the primary and secondary object are identical
  */
Vector3d
TwoBodyRotatingFrame::angularVelocity(double t) const
{
    StateVector state = m_secondary->state(t) - m_primary->state(t);
    if (state.position().isZero())
    {
        return Vector3d::Zero();
    }

    return state.position().cross(state.velocity()) / state.position().squaredNorm();
}

	static StateVector getStateAncestors( SLSF::State state ) {
		StateVector stateVector;
		stateVector.push_back( state );

		Udm::Object object = state.GetParent();
		while( object.type() == SLSF::State::meta ) {
			state = SLSF::State::Cast( object );
			stateVector.push_back( state );
			object = state.GetParent();
		}

		return stateVector;
	}

StateVector SourceGaussian::exec( double t){
    StateVector State;
    if(mTE){
        // Transverse electric mode
        double Hz = ScalarGaussian( t, mMean, mSigma);
        double Ey = c_eta0 * Hz;
        State.set(0,Ey,0,0,0,Hz);
    } else {
        // Transverse magnetic mode
        double Hy = ScalarGaussian( t, mMean, mSigma);
        double Ez = c_eta0 * Hy;
        State.set(0,0,Ez,0,Hy,0);
    }
    return State;
}

	static std::string getFullStateName( SLSF::State state ) {

		StateVector stateVector = getStateAncestors( state );

		std::string fullStateName;
		bool first = true;
		while( !stateVector.empty() ) {
			if ( first ) first = false;
			else         fullStateName += ".";
			fullStateName += stateVector.back().Name();
			stateVector.pop_back();
		}

		return fullStateName;
	}

// -------------------------------------------------------------------------------------------------
//  OperatorList::display(StateVector sv)
//
//  Use: Output a text visualisation of the operator list
//  Input: StateVector sv - the spin configuration
//  Output: none
// -------------------------------------------------------------------------------------------------
void OperatorList::display(StateVector sv)
{
    int N = sv.getSize();
    int L = getSize();

    const char* spins[2] = {"U", "D"};
    const char* ops[3] = {" ", "-", "="};

    for (int i = 0; i < L; i++) {
        cout << " ";
        for (int j = 0; j < N; j++)
            cout << spins[sv(j)] << " ";
        cout  << " (" << i << ")" << endl;

        int op = list->operators[i].op;
        if (op == 0) 
            cout << endl;
        else {
            int spin1 = list->operators[i].spin;
            int spin2 = spin1 + 1;
            if (spin2 > (N - 1))
                spin2 = 0;
            
            if (spin2 == 0) {
                cout << " " << ops[op];
                for (int k = 0; k < (2*N - 3); k++)
                    cout << " ";
                cout << ops[op] << endl;
            } else {
                for (int k = 0; k < (2*spin1 + 1); k++)
                    cout << " ";            
                for (int k = 0; k < 3; k++)
                    cout << ops[op];
                cout << endl;
            }

            // flip spins if off diagonal operator
            if (op == 2) {
                sv.flip(spin1);
                sv.flip(spin2);
            }            
        }
    }
    cout << " ";
    for (int j = 0; j < N; j++)
        cout << spins[sv(j)] << " ";
    cout << endl << endl << endl;
}

LQRController::UpdateGainsJob::UpdateGainsJob(LQRController * c, const StateMatrix & weights, const InputMatrix &input_weights, const StateVector & target)
: c_(c), target_(target)
{
  if(!c)
    return;
  ROS_DEBUG_STREAM("target is "<<target.transpose());
  const int n=weights.rows();
  const int m=input_weights.rows();
  
  input_offset_.resize(m,1);
  gains_.resize(n,n);
  
  //Computes a linearization around the target point
  StateMatrix A(n,n);
  InputMatrix B(n,m);
  if(!c_->model_->getLinearization(target, A, B, input_offset_))
  {
    ROS_ERROR("Failed to get linearization");
    c_=NULL;
    return;
  }
  ROS_DEBUG_STREAM("************ Obtained linearization **********\n[A]\n"<<A<<std::endl<<"[B]"<<std::endl<<B<<std::endl<<"[input offset]"<<std::endl<<input_offset_<<"\n*************");
  ROS_DEBUG("Sanity checks");
  ROS_ASSERT(LQR::isCommandable(A,B));
  // Compute new gains matrix
  ROS_DEBUG("Computing LQR gains matrix. Hold on to your seatbelts...");
  LQR::LQRDP<StateMatrix,InputMatrix,StateMatrix,InputMatrix>::runContinuous(A, B, weights, weights, input_weights, 0.01, 0.1, gains_);
  ROS_DEBUG_STREAM("Done. Computed gains:\n***********\n"<<gains_<<"\n**********\n");
  // Check validity
  ROS_DEBUG("Checking gains matrix...");
  bool test=LQR::isConverging(A,B,gains_);
  ROS_ASSERT(test);
  ROS_DEBUG("Valid gains matrix");
//   assert(test);
}