C++ StateVector::getData方法代码示例

/**
 * 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 testArm26() {
    ForwardTool forward("arm26_Setup_Forward.xml");
	forward.run();

	Storage results("Results/arm26_states.sto");
	Storage* standard = new Storage();
    string statesFileName("std_arm26_states.sto");
    forward.loadStatesStorage( statesFileName, standard );
	StateVector* state = results.getStateVector(0);
	double time = state->getTime();
	Array<double> data;
	data.setSize(state->getSize());
	standard->getDataAtTime(time, state->getSize(), data);
	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] << endl;
        ASSERT_EQUAL(data[j], state->getData()[j], 1.0e-3, __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
		cout << "ASSERT_EQUAL PASSED " << message.str();
    }

	int i = results.getSize()-1;
    state = results.getStateVector(i);
    time = state->getTime();
	data.setSize(state->getSize());
	standard->getDataAtTime(time, state->getSize(), data);
	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] << endl;
        ASSERT_EQUAL(data[j], state->getData()[j], 1.0e-3, __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
		cout << "ASSERT_EQUAL PASSED " << message.str();
	}
}

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

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

	// SET STATES
	setStates(aVector.getTime(),aVector.getSize(),&aVector.getData()[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;
	}
}

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

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

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

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

/**
 * From a potentially partial specification of the generalized coordinates,
 * form a complete storage of the generalized coordinates (q's) and
 * generalized speeds (u's).
 *
 * @param aQIn Storage containing the q's or a subset of the q's.  Rotational
 * q's should be in degrees.
 * @param rQComplete Storage containing all the q's.  If q's were not
 * in aQIn, the values are set to 0.0.  When a q is constrained, its value
 * is altered to be consistent with the constraint.  The caller is responsible
 * for deleting the memory associated with this storage.
 * @param rUComplete Storage containing all the u's.  The generalized speeds
 * are obtained by spline fitting the q's and differentiating the splines.
 * When a u is constrained, its value is altered to be consistent with the
 * constraint.  The caller is responsible for deleting the memory
 * associated with this storage.
 */
void SimbodyEngine::
formCompleteStorages( const SimTK::State& s, const OpenSim::Storage &aQIn,
    OpenSim::Storage *&rQComplete,OpenSim::Storage *&rUComplete) const
{
    int i;
    int nq = _model->getNumCoordinates();
    int nu = _model->getNumSpeeds();

    // Get coordinate file indices
    Array<string> columnLabels, speedLabels, coordStateNames;
    columnLabels.append("time");
    speedLabels = columnLabels;

    Array<int> index(-1,nq);
    const CoordinateSet& coordinateSet = _model->getCoordinateSet();
    int sizeCoordSet = coordinateSet.getSize();
    for(i=0;i<sizeCoordSet;i++) {
        Coordinate& coord = coordinateSet.get(i);
        string prefix = coord.getJoint().getName() + "/" + coord.getName() + "/";
        coordStateNames = coord.getStateVariableNames();
        columnLabels.append(prefix+coordStateNames[0]);
        speedLabels.append(prefix+coordStateNames[1]);
        int fix = aQIn.getStateIndex(coord.getName());
        if (fix < 0) {
            fix = aQIn.getStateIndex(columnLabels[i+1]);
        }

        index[i] = fix;
        if(index[i]<0) {
            string msg = "Model::formCompleteStorages(): WARNING- Did not find column ";
            msg += coordStateNames[0];
            msg += " in storage object.\n";
            cout << msg << endl;
        }
    }

    // Extract Coordinates
    double time;
    Array<double> data(0.0);
    Array<double> q(0.0,nq);
    Storage *qStore = new Storage();
    qStore->setInDegrees(aQIn.isInDegrees());
    qStore->setName("GeneralizedCoordinates");
    qStore->setColumnLabels(columnLabels);
    int size = aQIn.getSize();
    StateVector *vector;
    int j;
    for(i=0;i<size;i++) {
        vector = aQIn.getStateVector(i);
        data = vector->getData();
        time = vector->getTime();

        for(j=0;j<nq;j++) {
            q[j] = 0.0;
            if(index[j]<0) continue;
            q[j] = data[index[j]];
        }

        qStore->append(time,nq,&q[0]);
    }

    // Convert to radians
    if (aQIn.isInDegrees())
        convertDegreesToRadians(*qStore);


    // Compute generalized speeds
    GCVSplineSet tempQset(5,qStore);
    Storage *uStore = tempQset.constructStorage(1);

    // Compute constraints
    Array<double> qu(0.0,nq+nu);
    rQComplete = new Storage();
    rUComplete = new Storage();
    State constrainedState = s;
     _model->getMultibodySystem().realize(constrainedState, s.getSystemStage());
    for(i=0;i<size;i++) {
        qStore->getTime(i,time);
        qStore->getData(i,nq,&qu[0]);
        uStore->getData(i,nq,&qu[nq]);
        for (int j = 0; j < nq; j++) {
            Coordinate& coord = coordinateSet.get(j);
            coord.setValue(constrainedState, qu[j], false);
//.........这里部分代码省略.........