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;
  }
}
Exemplo n.º 2
0
/**
 * 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);
}
Exemplo n.º 3
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);
	}
Exemplo n.º 4
0
// -------------------------------------------------------------------------------------------------
//  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;
}
Exemplo n.º 5
0
/**
 * 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);
   }
}
Exemplo n.º 6
0
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;
	}
}
Exemplo n.º 7
0
/**
 * Copy constructor.
 */
StateVector::StateVector(const StateVector &aVector) :
	_data(0.0)
{
	// INITIAL VALUES
	setNull();

	// SET STATES
	setStates(aVector.getTime(),aVector.getSize(),&aVector.getData()[0]);
}
Exemplo n.º 8
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;
	}
Exemplo n.º 9
0
// -------------------------------------------------------------------------------------------------
//  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);                
        }
    }
} 
Exemplo n.º 10
0
/** 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();
}
Exemplo n.º 11
0
	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;
	}
Exemplo n.º 12
0
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;
}
Exemplo n.º 13
0
	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;
	}
Exemplo n.º 14
0
// -------------------------------------------------------------------------------------------------
//  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;
}
Exemplo n.º 15
0
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);
}
Exemplo n.º 16
0
bool LQRController::toStateVector(const std::vector<std::string> & names, const std::vector<double> & positions, const std::vector<double> & velocities, StateVector & v) const
{
  const int N = names.size();
  assert(model_);
  const int n = model_->states();
  if(v.rows() != n)
  {
    ROS_ERROR("Supplied state vector has wrong size");
    return false;
  }

  if((int)positions.size()!=N or (int)velocities.size()!=N)
  {
    ROS_ERROR("Incoherent data supplied");
    return false;
  }
  
  int count=1;
  for(int i=0;i<N;i++)
  {
    const int index=jointIndex(names.at(i));
    if(index>=0)
    {
      v(index,0)=positions.at(index);
      v(index+n/2,0)=velocities.at(index);
      count++;
    }
  }
  
  if(count!=n)
    ROS_INFO("The state was only partly specified");
  return true;
}
void
BCI2000OutputFormat::Initialize( const SignalProperties& inProperties,
                                 const StateVector& inStatevector )
{
  mInputProperties = inProperties;
  mStatevectorLength = inStatevector.Length();
}
Exemplo n.º 18
0
	/***
	 * You can get the basic block object by asking the Label to give you the basic block
	 * When you create a basic block, it also creates the parameter instructions for the current number of local variables.
	 * If you are tracking more than just local variables, you have to manually create a parameter instruction later.
	 */
	TessaVM::BasicBlock* TessaCreationApi::createNewBasicBlock() {
		TessaVM::BasicBlock* newBasicBlock = currentFunction->createNewBasicBlock();
		this->switchToBasicBlock(newBasicBlock);
		int localCount = currentFunction->getLocalCount();
		StateVector* beginStateVector = new (gc) StateVector(localCount, gc);

		for (int i = 0; i < localCount; i++) {
			beginStateVector->setLocal(i, new (gc) ParameterInstruction(NULL, newBasicBlock));
		}

		StateVector* endStateVector = beginStateVector->clone();

		newBasicBlock->setBeginState(beginStateVector);
		newBasicBlock->setEndState(endStateVector);
		return newBasicBlock;
	}
Exemplo n.º 19
0
void addLoadToStorage(Storage &forceStore, SimTK::Vec3 force, SimTK::Vec3 point, SimTK::Vec3 torque)
{
	int nLoads = forceStore.getColumnLabels().getSize()/9;
	string labels[9] = { "forceX", "forceY", "forceZ", "pointX", "pointY", "pointZ","torqueX", "torqueY", "torqueZ"};
	char suffix[2];
	sprintf(suffix, "%d", nLoads); 
	
	Array<string> col_labels;
	col_labels.append("time");
	StateVector dataRow;
	dataRow.setTime(0);
	double data[9];
	for(int i = 0; i<9; i++){
		col_labels.append(labels[i]);
		if(i<3){
			data[i] = force[i]; continue;
		}
		else if(i<6){
			data[i] = point[i-3]; continue;
		}
		else
			data[i] = torque[i-6];
	}

	dataRow.setStates(0, 9, data);

	Storage *forces = NULL;
	Storage tempStore;

	if(nLoads == 0)
		forces = &forceStore;
	else if (nLoads > 0)
		forces = &tempStore;
	else
		throw OpenSim::Exception("addLoadToStorage: ERROR");

	forces->setColumnLabels(col_labels);
	forces->append(dataRow);
	dataRow.setTime(1.0);
	forces->append(dataRow);
	dataRow.setTime(2.0);
	forces->append(dataRow);

	if (nLoads > 0)
		forces->addToRdStorage(forceStore, 0.0, 1.0);
}
Exemplo n.º 20
0
/** Get the orientation of the frame at the specified time. The frame's orientation
  * is undefined whenever one or more of the following is true:
  *  - the state of either the primary or secondary object is undefined
  *  - the positions of the primary and secondary object are identical
  *  - the secondary is stationary with respect to the primary
  *  - the position and velocity vectors are exactly aligned
  */
Quaterniond
TwoBodyRotatingFrame::orientation(double t) const
{
    StateVector state = m_secondary->state(t) - m_primary->state(t);
    if (state.position().isZero() || state.velocity().isZero())
    {
        return Quaterniond::Identity();
    }

    // Compute the axes of the two body rotating frame,
    // convert this to a matrix, then derive a quaternion
    // from this matrix.

    // x-axis points in direction from the primary to the secondary
    Vector3d xAxis = state.position().normalized();
    Vector3d v = state.velocity().normalized();

    Vector3d zAxis;
    if (m_velocityAlligned) {
        // z-axis normal to both the x-axis and the velocity vector
        zAxis = xAxis.cross(v);
        if (zAxis.isZero())
        {
            return Quaterniond::Identity();
        }
    }
    else {
        // z-axis normal to both the x-axis and the z axis of the primary
        zAxis = xAxis.cross(m_primary->orientation(t) * Vector3d::UnitX());
        if (zAxis.isZero())
        {
            return Quaterniond::Identity();
        }
    }

    zAxis.normalize();
    Vector3d yAxis = zAxis.cross(xAxis);

    Matrix3d m;
    m << xAxis, yAxis, zAxis;

    return Quaterniond(m);
}
Exemplo n.º 21
0
void copyStates( ESMoL::Stateflow inputStateflow, ESMoL::Stateflow outputStateflow ) {

	getConnectorRefList().clear();
	getTransConnectorMap().clear();

	StateVector stateVector = inputStateflow.State_kind_children();
	for( StateVector::iterator stvItr = stateVector.begin() ; stvItr != stateVector.end() ; ++stvItr ) {
		ESMoL::State inputState = *stvItr;
		ESMoL::State outputState = ESMoL::State::Create( outputStateflow );
		copyState( inputState, outputState );
	}
	for( ConnectorRefList::iterator crlItr = getConnectorRefList().begin() ; crlItr != getConnectorRefList().end() ; ++crlItr ) {
		ESMoL::ConnectorRef inputConnectorRef = *crlItr;
		ESMoL::TransConnector inputTransConnector = inputConnectorRef.ref();
		if ( inputTransConnector == Udm::null ) {
			std::cerr << "Warning:  ConnectorRef is a null reference!" << std::endl;
			continue;
		}

		TransConnectorMap::iterator tcmItr = getTransConnectorMap().find( inputConnectorRef );
		if ( tcmItr == getTransConnectorMap().end() ) {
			std::cerr << "Warning:  ConnectorRef has no copy in TransConnectorMap" << std::endl;
			continue;
		}
		ESMoL::TransConnector outputTransConnector = tcmItr->second;
		if ( outputTransConnector.type() != ESMoL::ConnectorRef::meta ) {
			std::cerr << "Warning:  TransConnector should be a ConnectorRef" << std::endl;
			continue;
		}
		ESMoL::ConnectorRef outputConnectorRef = ESMoL::ConnectorRef::Cast( outputTransConnector );

		tcmItr = getTransConnectorMap().find( inputTransConnector );
		if ( tcmItr == getTransConnectorMap().end() ) {
			std::cerr << "Warning:  TransConnector has no copy in TransConnectorMap" << std::endl;
			continue;
		}
		outputTransConnector = tcmItr->second;

		outputConnectorRef.ref() = outputTransConnector;
	}
}
Exemplo n.º 22
0
void
EDFFileWriterBase::PutBlock( const GenericSignal& inSignal, const StateVector& inStatevector )
{
  for( int i = 0; i < inSignal.Channels(); ++i )
    for( int j = 0; j < inSignal.Elements(); ++j )
      GDF::Num<T>( inSignal( i, j ) ).WriteToStream( OutputStream() );
  for( size_t i = 0; i < mStateNames.size(); ++i )
    GDF::PutField< GDF::Num<GDF::int16> >(
      OutputStream(),
      inStatevector.StateValue( mStateNames[ i ] )
    );
}
Exemplo n.º 23
0
bool StateVectorLimiter<StateType>::testState(const StateVector<StateType> &_stateVec) const
{
	bool result = true;

	if((_stateVec.size() != mUpper.size()) |(_stateVec.size() != mLower.size()))
	{
		std::cout << "incompatible dimensions. can't evaluate state limits\n";
		return false;
	}

	for(unsigned i=0; i < _stateVec.size(); i++)
	{
		bool valid = ((mLower(i) <= _stateVec[i]) & (mUpper(i) >= _stateVec[i]));
		result &= valid;
		std::string msg = valid ? "ok" : "error";
		std::cout << i << ". value=" << _stateVec[i] << "[" << mLower(i) << " ; " << mUpper(i)
				  << msg << "\n";

	}
	return result;
}
Exemplo n.º 24
0
StateVector BoundaryModuleDrivenBase::getBoundary( const StateVector& Reference, const char dim, const double t){
    // TODO Make the boundary both transmissive and driven.
    //      As it stands, it is possible to generate instabilities when backscattered waves meet the boundary.
    (void)Reference;
    double driventerm = getDrivenTerm(t);
    StateVector Boundary;

    // Set to zero
    Boundary = ZEROSTATE;

    // Calculate driven part
    double electric, magnetic;
    magnetic = driventerm;
    electric = c_eta0 * magnetic;

    // Add to boundary
    switch(dim){
        case 'x':
            switch(mPOL){
                case TE: 
                    Boundary.Ey() += electric;
                    Boundary.Hz() += magnetic;
                    break;
                case TM:
                    Boundary.Ez() += electric;
                    Boundary.Hy() += magnetic;
                    break;
            }
            break;
        case 'y':
            switch(mPOL){
                case TE: 
                    Boundary.Ex() += electric;
                    Boundary.Hz() += magnetic;
                    break;
                case TM:
                    Boundary.Ez() += electric;
                    Boundary.Hx() += magnetic;
                    break;
            }
            break;
    }
    
    return Boundary;
}
Exemplo n.º 25
0
void testPendulum() {
	ForwardTool forward("pendulum_Setup_Forward.xml");
	forward.run();
	Storage storage("Results/pendulum_states.sto");
	ASSERT(storage.getFirstTime() == 0.0);
	ASSERT(storage.getLastTime() == 1.0);

	// Since the pendulum is only swinging through small angles, it should be very
	// close to a simple harmonic oscillator.
	double previousTime = -1.0;
	double amp = SimTK::Pi/20;
	double k = sqrt(9.80665000/0.5);
	for (int j = 0; j < storage.getSize(); ++j) {
		StateVector* state = storage.getStateVector(j);
		double time = state->getTime();
		ASSERT(time > previousTime);
		previousTime = time;
		ASSERT_EQUAL(-amp*cos(k*time), state->getData()[0], 1.0e-2);
		ASSERT_EQUAL(amp*k*sin(k*time), state->getData()[1],1.0e-2);
	}
	ASSERT(previousTime == 1.0);
}
Exemplo n.º 26
0
int main()
{
   cout << endl;
   cout << "-----------------------------" << endl;
   cout << "-    Propagation Example    -" << endl;
   cout << "-----------------------------" << endl;
   cout << endl;

   auto propagator = otl::keplerian::LagrangianPropagator();
   
    // Setup inputs
    StateVector stateVector;
    stateVector.position = Vector3d(1131.340, -2282.343, 6672.423);  // Absolute position (km)
    stateVector.velocity = Vector3d(-5.64305, 4.30333, 2.42879);     // Absolute velocity (km/s)
    double mu = ASTRO_MU_EARTH;                                      // Gravitational parameter of Earth
    Time timeDelta = Time::Minutes(40.0);                            // Propagate forward 40 minutes

    cout << "-----------------------------" << endl;
    cout << "-           Input           -" << endl;
    cout << "-----------------------------" << endl;
    cout << "Initial state vector:" << endl << stateVector.ToDetailedString("  ") << endl;
    cout << "Propagation time:" << endl << timeDelta.ToDetailedString("  ") << endl;
   
    auto initialStateVector = stateVector;
   
    // Propagate the state vector forwards in time
    auto finalStateVector = propagator.PropagateStateVector(initialStateVector, mu, timeDelta);
   
    // Now propagate backwards in time to verify we end up where we started
    auto initialStateVector2 = propagator.PropagateStateVector(finalStateVector, mu, -timeDelta);
   
    cout << "-----------------------------" << endl;
    cout << "-          Output           -" << endl;
    cout << "-----------------------------" << endl;
    cout << "Final state vector:" << endl << finalStateVector.ToDetailedString("  ") << endl;
    cout << "Initial state vector after backwards propgation:" << endl << initialStateVector2.ToDetailedString("  ") << endl;

   return 0;
}
Exemplo n.º 27
0
void testGait2354WithController() {
    ForwardTool forward("subject01_Setup_Forward_Controller.xml");
	forward.run();
	Storage results("ResultsCorrectionController/subject01_states.sto");
	Storage* standard = new Storage();
    string statesFileName("std_subject01_walk1_states.sto");
    forward.loadStatesStorage( statesFileName, standard );

	Array<double> data;
	int i = results.getSize() - 1;
	StateVector* state = results.getStateVector(i);
	double time = state->getTime();
	data.setSize(state->getSize());
	standard->getDataAtTime(time, state->getSize(), data);
	int nc = forward.getModel().getNumCoordinates();
	for (int j = 0; j < nc; ++j) {      
	    stringstream message;
		message << "t=" << time <<" state# "<< j << " " << standard->getColumnLabels()[j+1] << " std=" << data[j] <<"  computed=" << state->getData()[j];
        ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
		cout << "ASSERT_EQUAL PASSED " << message.str() << endl;
	}
}
Exemplo n.º 28
0
/**
 * 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());

    // NUMBER OF Forces
    const ForceSet& forces = _model->getForceSet(); // This does not contain gravity
    int nf = forces.getSize();

    for(int i=0;i<nf;i++) {
        // If body force we need to record six values for torque+force
        // If muscle we record one scalar
        OpenSim::Force& nextForce = (OpenSim::Force&)forces[i];
        if (nextForce.isDisabled(s)) continue;
        Array<double> values = nextForce.getRecordValues(s);
        nextRow.getData().append(values);
    }

    if(_includeConstraintForces){
        // NUMBER OF Constraints
        const ConstraintSet& constraints = _model->getConstraintSet(); // This does not contain gravity
        int nc = constraints.getSize();

        for(int i=0;i<nc;i++) {
            OpenSim::Constraint& nextConstraint = (OpenSim::Constraint&)constraints[i];
            if (nextConstraint.isDisabled(s)) continue;
            Array<double> values = nextConstraint.getRecordValues(s);
            nextRow.getData().append(values);
        }
    }
    _forceStore.append(nextRow);

    return(0);
}
Exemplo n.º 29
0
void testPendulumExternalLoadWithPointInGround() {
	ForwardTool forward("pendulum_ext_point_in_ground_Setup_Forward.xml");cout << endl;
	forward.run();
	Storage results("Results/pendulum_ext_gravity_point_in_ground_states.sto");
	ASSERT(results.getFirstTime() == 0.0);
	ASSERT(results.getLastTime() == 1.0);

	Storage standard("Results/pendulum_states.sto");
 

	Array<double> data;
	int i = results.getSize() - 1;
	StateVector* state = results.getStateVector(i);
	double time = state->getTime();
	data.setSize(state->getSize());
	standard.getDataAtTime(time, state->getSize(), data);
	int nc = forward.getModel().getNumCoordinates();
	for (int j = 0; j < nc; ++j) {      
	    stringstream message;
		message << "t=" << time <<" state# "<< j << " " << standard.getColumnLabels()[j+1] << " std=" << data[j] <<"  computed=" << state->getData()[j];
        ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
		cout << "ASSERT_EQUAL PASSED " << message.str() << endl;
	}
}
Exemplo n.º 30
0
void testGait2354()
{
	ForwardTool forward("subject01_Setup_Forward.xml");
	forward.run();
	Storage results("Results/subject01_states.sto");
	Storage* standard = new Storage();
    string statesFileName("std_subject01_walk1_states.sto");
    forward.loadStatesStorage( statesFileName, standard );

	Array<double> data;
	StateVector* state = results.getStateVector(0);
	double time = state->getTime();
	data.setSize(state->getSize());
	standard->getDataAtTime(time, state->getSize(), data);

	// At initial time all states should be identical except for locked joints may vary slightly due to 
	// differences in OpenSim's integrator and SimTK's
	int nc = forward.getModel().getNumCoordinates();
	for (int j = 0; j < state->getSize(); ++j) {
		stringstream message;
		message << "t=" << time <<" state# "<< j << " " << standard->getColumnLabels()[j+1] << " std=" << data[j] <<"  computed=" << state->getData()[j];
        ASSERT_EQUAL(data[j], state->getData()[j], 1e-4, __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
		cout << "ASSERT_EQUAL PASSED " << message.str() << endl;;
	}

	int i = results.getSize()-1;
	state = results.getStateVector(i);
	time = state->getTime();
	standard->getDataAtTime(time, state->getSize(), data);

	// NOTE: Gait model is running forward open-loop. We cannot expect all the states to
	// be "bang on" and we expect a gradual drift in the coordinates.  Check to see that
	// coordinates haven't drifted too far off.
	for (int j = 0; j < nc; ++j) {
	    stringstream message;
		message << "t=" << time <<" state# "<< j << " " << standard->getColumnLabels()[j+1] << " std=" << data[j] <<"  computed=" << state->getData()[j];
        ASSERT_EQUAL(data[j], state->getData()[j], 4e-2, __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
		cout << "ASSERT_EQUAL PASSED " << message.str() << endl;
	}
}