/**
* 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);
coord.setSpeedValue(constrainedState, qu[nq+j]);
}
_model->assemble(constrainedState);
for (int j = 0; j < nq; j++) {
Coordinate& coord = coordinateSet.get(j);
qu[j] = coord.getValue(constrainedState);
qu[nq+j] = coord.getSpeedValue(constrainedState);
}
rQComplete->append(time,nq,&qu[0]);
rUComplete->append(time,nu,&qu[nq]);
}
delete qStore;
// Compute storage object for simulation
// Need to set column labels before converting rad->deg
rQComplete->setColumnLabels(columnLabels);
rUComplete->setColumnLabels(speedLabels);
// Convert back to degrees
convertRadiansToDegrees(*rQComplete);
convertRadiansToDegrees(*rUComplete);
}