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