void MainWindow::updatePlayButtons()
{
SimWorld* world = ui->glWidget->getWorld();
MarkerData* markerData = world->getMarkerData();
if (world->isPaused()) {
ui->playPauseSimButton->setIcon(QIcon(":/icons/data/media-playback-start-3.png"));
ui->playPauseSimButton->setChecked(false);
} else {
ui->playPauseSimButton->setIcon(QIcon(":/icons/data/media-playback-pause-3.png"));
ui->playPauseSimButton->setChecked(true);
}
if (markerData->isPaused()) {
ui->playPauseDataButton->setIcon(QIcon(":/icons/data/media-playback-start-3.png"));
ui->playPauseDataButton->setChecked(false);
} else {
ui->playPauseDataButton->setIcon(QIcon(":/icons/data/media-playback-pause-3.png"));
ui->playPauseDataButton->setChecked(true); }
if (!(world->isPaused()||markerData->isPaused())) {
ui->playAllButton->setIcon(QIcon(":/icons/data/media-playback-pause-3.png"));
ui->playAllButton->setChecked(true);
} else {
ui->playAllButton->setIcon(QIcon(":/icons/data/media-playback-start-3.png"));
ui->playAllButton->setChecked(false);
}
}
/**
* Measure the average distance between a marker pair in an experimental marker data.
*/
double ModelScaler::takeExperimentalMarkerMeasurement(const MarkerData& aMarkerData, const string& aName1, const string& aName2, const string& aMeasurementName) const
{
const Array<string>& experimentalMarkerNames = aMarkerData.getMarkerNames();
int marker1 = experimentalMarkerNames.findIndex(aName1);
int marker2 = experimentalMarkerNames.findIndex(aName2);
if (marker1 >= 0 && marker2 >= 0) {
int startIndex, endIndex;
if (_timeRange.getSize()<2)
throw Exception("ModelScaler::takeExperimentalMarkerMeasurement, time_range is unspecified.");
aMarkerData.findFrameRange(_timeRange[0], _timeRange[1], startIndex, endIndex);
double length = 0;
for(int i=startIndex; i<=endIndex; i++) {
Vec3 p1 = aMarkerData.getFrame(i).getMarker(marker1);
Vec3 p2 = aMarkerData.getFrame(i).getMarker(marker2);
length += (p2 - p1).norm();
}
return length/(endIndex-startIndex+1);
} else {
if (marker1 < 0)
cout << "___WARNING___: marker " << aName1 << " in " << aMeasurementName << " measurement not found in " << aMarkerData.getFileName() << endl;
if (marker2 < 0)
cout << "___WARNING___: marker " << aName2 << " in " << aMeasurementName << " measurement not found in " << aMarkerData.getFileName() << endl;
return SimTK::NaN;
}
}
void MainWindow::playPauseData(bool play)
{
MarkerData* markerData = ui->glWidget->getWorld()->getMarkerData();
if (markerData->isPaused() != play) return;
markerData->setPaused(!play);
updatePlayButtons();
}
/**
* Set the local offset of each non-fixed marker so that in the model's
* current pose the marker coincides with the marker's global position
* in the passed-in MarkerData.
*
* @param aModel the model to use
* @param aPose the static-pose marker cloud to get the marker locations from
*/
void MarkerPlacer::moveModelMarkersToPose(SimTK::State& s, Model& aModel,
MarkerData& aPose) const
{
aPose.averageFrames(0.01);
const MarkerFrame &frame = aPose.getFrame(0);
// const SimbodyEngine& engine = aModel.getSimbodyEngine();
MarkerSet& markerSet = aModel.updMarkerSet();
int i;
for (i = 0; i < markerSet.getSize(); i++)
{
Marker& modelMarker = markerSet.get(i);
if (true/*!modelMarker.getFixed()*/)
{
int index = aPose.getMarkerIndex(modelMarker.getName());
if (index >= 0)
{
Vec3 globalMarker = frame.getMarker(index);
if (!globalMarker.isNaN())
{
Vec3 pt, pt2;
Vec3 globalPt = globalMarker;
double conversionFactor = aPose.getUnits().convertTo(aModel.getLengthUnits());
pt = conversionFactor*globalPt;
pt2 = modelMarker.getParentFrame().findLocationInAnotherFrame(s, pt, aModel.getGround());
modelMarker.set_location(pt2);
}
else
{
cout << "___WARNING___: marker " << modelMarker.getName() << " does not have valid coordinates in " << aPose.getFileName() << endl;
cout << " It will not be moved to match location in marker file." << endl;
}
}
}
}
cout << "Moved markers in model " << aModel.getName() << " to match locations in marker file " << aPose.getFileName() << endl;
}
/* A convenience method to populate MarkersReference from MarkerData **/
void MarkersReference::populateFromMarkerData(const MarkerData& aMarkerData)
{
const Array<std::string> &tempNames = aMarkerData.getMarkerNames();
int nm = tempNames.getSize();
// empty any lingering names and weights
_markerNames.clear();
_weights.clear();
// pre-allocate arrays to the number of markers in the file with default weightings
_markerNames.assign(nm, "");
_weights.assign(nm, get_default_weight());
for (int i = 0; i < nm; i++) {
const std::string &name = tempNames[i];
_markerNames[i] = name;
}
// Names must be assigned before weights can be updated
updateInternalWeights();
}
/**
* This method creates a SimmMotionTrial instance with the markerFile and
* timeRange parameters. It also creates a Storage instance with the
* coordinateFile parameter. Then it updates the coordinates and markers in
* the model, if specified. Then it does IK to fit the model to the static
* pose. Then it uses the current model pose to relocate all non-fixed markers
* according to their locations in the SimmMotionTrial. Then it writes the
* output files selected by the user.
*
* @param aModel the model to use for the marker placing process.
* @return Whether the marker placing process was successful or not.
*/
bool MarkerPlacer::processModel(Model* aModel,
const string& aPathToSubject) const {
if(!getApply()) return false;
cout << endl << "Step 3: Placing markers on model" << endl;
if (_timeRange.getSize()<2)
throw Exception("MarkerPlacer::processModel, time_range is unspecified.");
/* Load the static pose marker file, and average all the
* frames in the user-specified time range.
*/
MarkerData* staticPose = new MarkerData(aPathToSubject + _markerFileName);
staticPose->averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]);
staticPose->convertToUnits(aModel->getLengthUnits());
/* Delete any markers from the model that are not in the static
* pose marker file.
*/
aModel->deleteUnusedMarkers(staticPose->getMarkerNames());
// Construct the system and get the working state when done changing the model
SimTK::State& s = aModel->initSystem();
// Create references and WeightSets needed to initialize InverseKinemaicsSolver
Set<MarkerWeight> markerWeightSet;
_ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
// MarkersReference takes ownership of marker data (staticPose)
MarkersReference markersReference(staticPose, &markerWeightSet);
SimTK::Array_<CoordinateReference> coordinateReferences;
// Load the coordinate data
// create CoordinateReferences for Coordinate Tasks
FunctionSet *coordFunctions = NULL;
// bool haveCoordinateFile = false;
if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){
Storage coordinateValues(aPathToSubject + _coordinateFileName);
aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
// haveCoordinateFile = true;
coordFunctions = new GCVSplineSet(5,&coordinateValues);
}
int index = 0;
for(int i=0; i< _ikTaskSet.getSize(); i++){
IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
if (coordTask && coordTask->getApply()){
std::unique_ptr<CoordinateReference> coordRef{};
if(coordTask->getValueType() == IKCoordinateTask::FromFile){
index = coordFunctions->getIndex(coordTask->getName(), index);
if(index >= 0){
coordRef.reset(new CoordinateReference(coordTask->getName(),coordFunctions->get(index)));
}
}
else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){
Constant reference(Constant(coordTask->getValue()));
coordRef.reset(new CoordinateReference(coordTask->getName(), reference));
}
else{ // assume it should be held at its current/default value
double value = aModel->getCoordinateSet().get(coordTask->getName()).getValue(s);
Constant reference = Constant(value);
coordRef.reset(new CoordinateReference(coordTask->getName(), reference));
}
if(coordRef == NULL)
throw Exception("MarkerPlacer: value for coordinate "+coordTask->getName()+" not found.");
// We have a valid coordinate reference so now set its weight according to the task
coordRef->setWeight(coordTask->getWeight());
coordinateReferences.push_back(*coordRef);
}
}
double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();
InverseKinematicsSolver ikSol(*aModel, markersReference,
coordinateReferences, constraintWeight);
ikSol.assemble(s);
// Call realize Position so that the transforms are updated and markers can be moved correctly
aModel->getMultibodySystem().realize(s, SimTK::Stage::Position);
// Report marker errors to assess the quality
int nm = markerWeightSet.getSize();
SimTK::Array_<double> squaredMarkerErrors(nm, 0.0);
SimTK::Array_<Vec3> markerLocations(nm, Vec3(0));
double totalSquaredMarkerError = 0.0;
double maxSquaredMarkerError = 0.0;
int worst = -1;
// Report in the same order as the marker tasks/weights
ikSol.computeCurrentSquaredMarkerErrors(squaredMarkerErrors);
for(int j=0; j<nm; ++j){
totalSquaredMarkerError += squaredMarkerErrors[j];
if(squaredMarkerErrors[j] > maxSquaredMarkerError){
maxSquaredMarkerError = squaredMarkerErrors[j];
worst = j;
}
}
cout << "Frame at (t=" << s.getTime() << "):\t";
cout << "total squared error = " << totalSquaredMarkerError;
cout << ", marker error: RMS=" << sqrt(totalSquaredMarkerError/nm);
cout << ", max=" << sqrt(maxSquaredMarkerError) << " (" << ikSol.getMarkerNameForIndex(worst) << ")" << endl;
/* Now move the non-fixed markers on the model so that they are coincident
* with the measured markers in the static pose. The model is already in
* the proper configuration so the coordinates do not need to be changed.
*/
if(_moveModelMarkers) moveModelMarkersToPose(s, *aModel, *staticPose);
_outputStorage.reset();
// Make a storage file containing the solved states and markers for display in GUI.
Storage motionData;
StatesReporter statesReporter(aModel);
statesReporter.begin(s);
_outputStorage.reset(new Storage(statesReporter.updStatesStorage()));
_outputStorage->setName("static pose");
//_outputStorage->print("statesReporterOutput.sto");
Storage markerStorage;
staticPose->makeRdStorage(*_outputStorage);
_outputStorage->getStateVector(0)->setTime(s.getTime());
statesReporter.updStatesStorage().addToRdStorage(*_outputStorage, s.getTime(), s.getTime());
//_outputStorage->print("statesReporterOutputWithMarkers.sto");
if(_printResultFiles) {
if (!_outputModelFileNameProp.getValueIsDefault())
{
aModel->print(aPathToSubject + _outputModelFileName);
cout << "Wrote model file " << _outputModelFileName << " from model " << aModel->getName() << endl;
}
if (!_outputMarkerFileNameProp.getValueIsDefault())
{
aModel->writeMarkerFile(aPathToSubject + _outputMarkerFileName);
cout << "Wrote marker file " << _outputMarkerFileName << " from model " << aModel->getName() << endl;
}
if (!_outputMotionFileNameProp.getValueIsDefault())
{
_outputStorage->print(aPathToSubject + _outputMotionFileName,
"w", "File generated from solving marker data for model "+aModel->getName());
}
}
return true;
}
/**
* This method scales a model based on user-specified XYZ body scale factors
* and/or a set of marker-to-marker distance measurements.
*
* @param aModel the model to scale.
* @param aSubjectMass the final mass of the model after scaling.
* @return Whether the scaling process was successful or not.
*/
bool ModelScaler::processModel(Model* aModel, const string& aPathToSubject, double aSubjectMass)
{
if (!getApply()) return false;
int i;
ScaleSet theScaleSet;
Vec3 unity(1.0);
cout << endl << "Step 2: Scaling generic model" << endl;
ComponentList<PhysicalFrame> segments
= aModel->getComponentList<PhysicalFrame>();
ComponentList<PhysicalFrame>::const_iterator it = segments.begin();
/* Make a scale set with a Scale for each physical frame.
* Initialize all factors to 1.0.
*/
for (; it != segments.end(); ++it) {
Scale* segmentScale = new Scale();
segmentScale->setSegmentName(it->getName());
segmentScale->setScaleFactors(unity);
segmentScale->setApply(true);
theScaleSet.adoptAndAppend(segmentScale);
}
SimTK::State& s = aModel->initSystem();
aModel->getMultibodySystem().realize(s, SimTK::Stage::Position);
try
{
/* Make adjustments to theScaleSet, in the user-specified order. */
for (i = 0; i < _scalingOrder.getSize(); i++)
{
/* For measurements, measure the distance between a pair of markers
* in the model, and in the static pose. The latter divided by the
* former is the scale factor. Put that scale factor in theScaleSet,
* using the body/axis names specified in the measurement to
* determine in what place[s] to put the factor.
*/
if (_scalingOrder[i] == "measurements")
{
/* Load the static pose marker file, and convert units.
*/
MarkerData *markerData = 0;
if(!_markerFileName.empty() && _markerFileName!=PropertyStr::getDefaultStr()) {
markerData = new MarkerData(aPathToSubject + _markerFileName);
markerData->convertToUnits(aModel->getLengthUnits());
}
/* Now take and apply the measurements. */
for (int j = 0; j < _measurementSet.getSize(); j++)
{
if (_measurementSet.get(j).getApply())
{
if(!markerData)
throw Exception("ModelScaler.processModel: ERROR- "+_markerFileNameProp.getName()+
" not set but measurements are used",__FILE__,__LINE__);
double scaleFactor = computeMeasurementScaleFactor(s,*aModel, *markerData, _measurementSet.get(j));
if (!SimTK::isNaN(scaleFactor))
_measurementSet.get(j).applyScaleFactor(scaleFactor, theScaleSet);
else
cout << "___WARNING___: " << _measurementSet.get(j).getName() << " measurement not used to scale " << aModel->getName() << endl;
}
}
}
/* For manual scales, just copy the XYZ scale factors from
* the manual scale into theScaleSet.
*/
else if (_scalingOrder[i] == "manualScale")
{
for (int j = 0; j < _scaleSet.getSize(); j++)
{
if (_scaleSet[j].getApply())
{
const string& bodyName = _scaleSet[j].getSegmentName();
Vec3 factors(1.0);
_scaleSet[j].getScaleFactors(factors);
for (int k = 0; k < theScaleSet.getSize(); k++)
{
if (theScaleSet[k].getSegmentName() == bodyName)
theScaleSet[k].setScaleFactors(factors);
}
}
}
}
else
{
throw Exception("ModelScaler: ERR- Unrecognized string '"+_scalingOrder[i]+"' in "+_scalingOrderProp.getName()+" property (expecting 'measurements' or 'manualScale').",__FILE__,__LINE__);
}
}
/* Now scale the model. */
aModel->scale(s, theScaleSet, aSubjectMass, _preserveMassDist);
if(_printResultFiles) {
std::string savedCwd = IO::getCwd();
IO::chDir(aPathToSubject);
if (!_outputModelFileNameProp.getValueIsDefault())
{
if (aModel->print(_outputModelFileName))
cout << "Wrote model file " << _outputModelFileName << " from model " << aModel->getName() << endl;
}
if (!_outputScaleFileNameProp.getValueIsDefault())
{
if (theScaleSet.print(_outputScaleFileName))
cout << "Wrote scale file " << _outputScaleFileName << " for model " << aModel->getName() << endl;
}
IO::chDir(savedCwd);
}
}
catch (const Exception& x)
{
x.print(cout);
return false;
}
return true;
}
/**
* This method creates a SimmMotionTrial instance with the markerFile and
* timeRange parameters. It also creates a Storage instance with the
* coordinateFile parameter. Then it updates the coordinates and markers in
* the model, if specified. Then it does IK to fit the model to the static
* pose. Then it uses the current model pose to relocate all non-fixed markers
* according to their locations in the SimmMotionTrial. Then it writes the
* output files selected by the user.
*
* @param aModel the model to use for the marker placing process.
* @return Whether the marker placing process was successful or not.
*/
bool MarkerPlacer::processModel(Model* aModel,
const string& aPathToSubject) const {
if(!getApply()) return false;
cout << endl << "Step 3: Placing markers on model" << endl;
if (_timeRange.getSize()<2)
throw Exception("MarkerPlacer::processModel, time_range is unspecified.");
/* Load the static pose marker file, and average all the
* frames in the user-specified time range.
*/
TimeSeriesTableVec3 staticPoseTable{aPathToSubject + _markerFileName};
const auto& timeCol = staticPoseTable.getIndependentColumn();
// Users often set a time range that purposely exceeds the range of
// their data with the mindset that all their data will be used.
// To allow for that, we have to narrow the provided range to data
// range, since the TimeSeriesTable will correctly throw that the
// desired time exceeds the data range.
if (_timeRange[0] < timeCol.front())
_timeRange[0] = timeCol.front();
if (_timeRange[1] > timeCol.back())
_timeRange[1] = timeCol.back();
const auto avgRow = staticPoseTable.averageRow(_timeRange[0],
_timeRange[1]);
for(size_t r = staticPoseTable.getNumRows(); r-- > 0; )
staticPoseTable.removeRowAtIndex(r);
staticPoseTable.appendRow(_timeRange[0], avgRow);
OPENSIM_THROW_IF(!staticPoseTable.hasTableMetaDataKey("Units"),
Exception,
"MarkerPlacer::processModel -- Marker file does not have "
"'Units'.");
Units
staticPoseUnits{staticPoseTable.getTableMetaData<std::string>("Units")};
double scaleFactor = staticPoseUnits.convertTo(aModel->getLengthUnits());
OPENSIM_THROW_IF(SimTK::isNaN(scaleFactor),
Exception,
"Model has unspecified units.");
if(std::fabs(scaleFactor - 1) >= SimTK::Eps) {
for(unsigned r = 0; r < staticPoseTable.getNumRows(); ++r)
staticPoseTable.updRowAtIndex(r) *= scaleFactor;
staticPoseUnits = aModel->getLengthUnits();
staticPoseTable.removeTableMetaDataKey("Units");
staticPoseTable.addTableMetaData("Units",
staticPoseUnits.getAbbreviation());
}
MarkerData* staticPose = new MarkerData(aPathToSubject + _markerFileName);
staticPose->averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]);
staticPose->convertToUnits(aModel->getLengthUnits());
/* Delete any markers from the model that are not in the static
* pose marker file.
*/
aModel->deleteUnusedMarkers(staticPose->getMarkerNames());
// Construct the system and get the working state when done changing the model
SimTK::State& s = aModel->initSystem();
s.updTime() = _timeRange[0];
// Create references and WeightSets needed to initialize InverseKinemaicsSolver
Set<MarkerWeight> markerWeightSet;
_ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
// MarkersReference takes ownership of marker data (staticPose)
MarkersReference markersReference(staticPoseTable, &markerWeightSet);
SimTK::Array_<CoordinateReference> coordinateReferences;
// Load the coordinate data
// create CoordinateReferences for Coordinate Tasks
FunctionSet *coordFunctions = NULL;
// bool haveCoordinateFile = false;
if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){
Storage coordinateValues(aPathToSubject + _coordinateFileName);
aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
// haveCoordinateFile = true;
coordFunctions = new GCVSplineSet(5,&coordinateValues);
}
int index = 0;
for(int i=0; i< _ikTaskSet.getSize(); i++){
IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
if (coordTask && coordTask->getApply()){
std::unique_ptr<CoordinateReference> coordRef{};
if(coordTask->getValueType() == IKCoordinateTask::FromFile){
index = coordFunctions->getIndex(coordTask->getName(), index);
if(index >= 0){
coordRef.reset(new CoordinateReference(coordTask->getName(),coordFunctions->get(index)));
}
}
else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){
Constant reference(Constant(coordTask->getValue()));
coordRef.reset(new CoordinateReference(coordTask->getName(), reference));
}
else{ // assume it should be held at its current/default value
double value = aModel->getCoordinateSet().get(coordTask->getName()).getValue(s);
Constant reference = Constant(value);
coordRef.reset(new CoordinateReference(coordTask->getName(), reference));
}
if(coordRef == NULL)
throw Exception("MarkerPlacer: value for coordinate "+coordTask->getName()+" not found.");
// We have a valid coordinate reference so now set its weight according to the task
coordRef->setWeight(coordTask->getWeight());
coordinateReferences.push_back(*coordRef);
}
}
double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();
InverseKinematicsSolver ikSol(*aModel, markersReference,
coordinateReferences, constraintWeight);
ikSol.assemble(s);
// Call realize Position so that the transforms are updated and markers can be moved correctly
aModel->getMultibodySystem().realize(s, SimTK::Stage::Position);
// Report marker errors to assess the quality
int nm = markerWeightSet.getSize();
SimTK::Array_<double> squaredMarkerErrors(nm, 0.0);
SimTK::Array_<Vec3> markerLocations(nm, Vec3(0));
double totalSquaredMarkerError = 0.0;
double maxSquaredMarkerError = 0.0;
int worst = -1;
// Report in the same order as the marker tasks/weights
ikSol.computeCurrentSquaredMarkerErrors(squaredMarkerErrors);
for(int j=0; j<nm; ++j){
totalSquaredMarkerError += squaredMarkerErrors[j];
if(squaredMarkerErrors[j] > maxSquaredMarkerError){
maxSquaredMarkerError = squaredMarkerErrors[j];
worst = j;
}
}
cout << "Frame at (t=" << s.getTime() << "):\t";
cout << "total squared error = " << totalSquaredMarkerError;
cout << ", marker error: RMS=" << sqrt(totalSquaredMarkerError/nm);
cout << ", max=" << sqrt(maxSquaredMarkerError) << " (" << ikSol.getMarkerNameForIndex(worst) << ")" << endl;
/* Now move the non-fixed markers on the model so that they are coincident
* with the measured markers in the static pose. The model is already in
* the proper configuration so the coordinates do not need to be changed.
*/
if(_moveModelMarkers) moveModelMarkersToPose(s, *aModel, *staticPose);
_outputStorage.reset();
// Make a storage file containing the solved states and markers for display in GUI.
Storage motionData;
StatesReporter statesReporter(aModel);
statesReporter.begin(s);
_outputStorage.reset(new Storage(statesReporter.updStatesStorage()));
_outputStorage->setName("static pose");
_outputStorage->getStateVector(0)->setTime(s.getTime());
if(_printResultFiles) {
std::string savedCwd = IO::getCwd();
IO::chDir(aPathToSubject);
try { // writing can throw an exception
if (_outputModelFileNameProp.isValidFileName()) {
aModel->print(aPathToSubject + _outputModelFileName);
cout << "Wrote model file " << _outputModelFileName <<
" from model " << aModel->getName() << endl;
}
if (_outputMarkerFileNameProp.isValidFileName()) {
aModel->writeMarkerFile(aPathToSubject + _outputMarkerFileName);
cout << "Wrote marker file " << _outputMarkerFileName <<
" from model " << aModel->getName() << endl;
}
if (_outputMotionFileNameProp.isValidFileName()) {
_outputStorage->print(aPathToSubject + _outputMotionFileName,
"w", "File generated from solving marker data for model "
+ aModel->getName());
}
} // catch the exception so we can reset the working directory
catch (std::exception& ex) {
IO::chDir(savedCwd);
OPENSIM_THROW_FRMOBJ(Exception, ex.what());
}
IO::chDir(savedCwd);
}
return true;
}
