/** * Set the velocity functions for the tasks. Functions are set based on the * correspondence of the function and the task. For example, * a task with the name "x" will search for a function or functions * with the name "x". For tasks that require 3 functions, such * as CMC_Point tasks, the assumption is that there will be three * consecutive functions named "x" in the function set. If the correct * number of functions is not found, the task is disabled. * * @param aFuncSet Function set. * @return Pointer to the previous function set. */ void CMC_TaskSet:: setFunctionsForVelocity(FunctionSet &aFuncSet) { // LOOP THROUGH TRACK OBJECTS int i,j,iFunc=0; int nTrk; string name; Function *f[3]; const CoordinateSet& coords = getModel()->getCoordinateSet(); for(i=0;i<getSize();i++) { // OBJECT TrackingTask& ttask = get(i); // If CMC_Task process same way as pre 2.0.2 if (dynamic_cast<CMC_Task*>(&ttask)==NULL) continue; CMC_Task& task = dynamic_cast<CMC_Task&>(ttask); // NAME name = task.getName(); if(name.empty()) continue; const Coordinate& coord = coords.get(name); // FIND FUNCTION(S) f[0] = f[1] = f[2] = NULL; nTrk = task.getNumTaskFunctions(); iFunc = aFuncSet.getIndex(coord.getSpeedName(),iFunc); if (iFunc < 0){ name = coord.getJoint().getName() + "/" + coord.getSpeedName(); iFunc = aFuncSet.getIndex(name, iFunc); if (iFunc < 0){ string msg = "CMC_TaskSet::setFunctionsForVelocity: function for task '"; msg += name + " not found."; throw Exception(msg); } } for(j=0;j<nTrk;j++) { try { f[j] = &aFuncSet.get(iFunc); } catch(const Exception& x) { x.print(cout); } if(f[j]==NULL) break; } task.setTaskFunctionsForVelocity(f[0],f[1],f[2]); } }
/** * 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) { if(!getApply()) return false; cout << endl << "Step 3: Placing markers on model" << endl; /* Load the static pose marker file, and average all the * frames in the user-specified time range. */ MarkerData staticPose(aPathToSubject + _markerFileName); if (_timeRange.getSize()<2) throw Exception("MarkerPlacer::processModel, time_range is unspecified."); 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 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()){ CoordinateReference *coordRef = NULL; if(coordTask->getValueType() == IKCoordinateTask::FromFile){ index = coordFunctions->getIndex(coordTask->getName(), index); if(index >= 0){ coordRef = new CoordinateReference(coordTask->getName(),coordFunctions->get(index)); } } else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){ Constant reference(Constant(coordTask->getValue())); coordRef = 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 = 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); if (_outputStorage!= NULL){ delete _outputStorage; } // Make a storage file containing the solved states and markers for display in GUI. Storage motionData; StatesReporter statesReporter(aModel); statesReporter.begin(s); _outputStorage = 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; }
/** * Set the functions for the tasks. Functions are set based on the * correspondence of the function and the task. For example, * a task with the name "x" will search for a function or functions * with the name "x". For tasks that require 3 functions, such * as CMC_Point tasks, the assumption is that there will be three * consecutive functions named "x" in the function set. If the correct * number of functions is not found, the task is disabled. * * @param aFuncSet Function set. * @return Pointer to the previous function set. */ void CMC_TaskSet:: setFunctions(FunctionSet &aFuncSet) { // LOOP THROUGH TRACK OBJECTS int i,j,iFunc=0; int nTrk; string name; Function *f[3]; for(i=0;i<getSize();i++) { // OBJECT TrackingTask& ttask = get(i); if (dynamic_cast<StateTrackingTask*>(&ttask)!=NULL) { StateTrackingTask& sTask = dynamic_cast<StateTrackingTask&>(ttask); if (aFuncSet.contains(sTask.getName())){ sTask.setTaskFunctions(&aFuncSet.get(sTask.getName())); } else{ cout << "State tracking task " << sTask.getName() << "has no data to track and will be ignored" << std::endl; } continue; } // If CMC_Task process same way as pre 2.0.2 if (dynamic_cast<CMC_Task*>(&ttask)==NULL) continue; CMC_Task& task = dynamic_cast<CMC_Task&>(ttask); // NAME name = task.getName(); if(name.empty()) continue; // FIND FUNCTION(S) f[0] = f[1] = f[2] = NULL; nTrk = task.getNumTaskFunctions(); iFunc = aFuncSet.getIndex(name,iFunc); if (iFunc < 0){ const Coordinate& coord = _model->getCoordinateSet().get(name); name = coord.getJoint().getName() + "/" + name + "/value"; iFunc = aFuncSet.getIndex(name, iFunc); if (iFunc < 0){ string msg = "CMC_TaskSet::setFunctionsForVelocity: function for task '"; msg += name + " not found."; throw Exception(msg); } } for(j=0;j<nTrk;j++) { try { f[j] = &aFuncSet.get(iFunc); } catch(const Exception& x) { x.print(cout); } if(f[j]==NULL) break; if(name == f[j]->getName()) { iFunc++; } else { f[j] = NULL; break; } } task.setTaskFunctions(f[0],f[1],f[2]); } }
/** * 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; }