Пример #1
0
/**
 * 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;
}
Пример #2
0
/**
 * 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;
}
Пример #3
0
/**
 * 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;
}