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