PolynomialPathFitter.cpp

/* -------------------------------------------------------------------------- *
 *                    OpenSim:  PolynomialPathFitter.cpp                      *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2026 Stanford University and the Authors                *
 * Author(s): Nicholas Bianco                                                 *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "PolynomialPathFitter.h"

#include <OpenSim/Actuators/ModelOperators.h>

#include <OpenSim/Common/CommonUtilities.h>
#include <OpenSim/Common/IO.h>
#include <OpenSim/Common/STOFileAdapter.h>
#include <OpenSim/Common/LatinHypercubeDesign.h>
#include <OpenSim/Common/MultivariatePolynomialFunction.h>
#include <OpenSim/Simulation/Control/PrescribedController.h>
#include <OpenSim/Simulation/Manager/Manager.h>

#include <atomic>
#include <future>

using namespace OpenSim;

namespace {

// Type alias for the moment arm map. The keys are the paths in the model
// and the values are vectors containing the (model) paths to coordinates on
// which the (wrapping) paths depend.
using MomentArmMap = std::unordered_map<std::string, std::vector<std::string>>;

// Type alias for the coordinate bounds and coordinate range maps. The keys are
// the coordinate paths in the model and the values are `SimTK::Vec2` objects
// containing the bounds or ranges for each coordinate.
using CoordinateMap = std::unordered_map<std::string, SimTK::Vec2>;

// A struct for containing validated settings based on the values provided by
// the user via the PolynomialPathFitter properties.
struct Settings {
    bool useStepwiseRegression = false;
    double momentArmTolerance = 1e-4;
    double pathLengthTolerance = 1e-4;
    int minimumPolynomialOrder = 2;
    int maximumPolynomialOrder = 6;
    int numParallelThreads = (int)std::thread::hardware_concurrency();
    SimTK::Vec2 globalCoordinateSamplingBounds{-10.0, 10.0};
    int numSamplesPerFrame = 25;
    bool useStochasticEvolutionaryLHS = false;
    bool includeMomentArmFunctions = false;
    bool includeLengtheningSpeedFunction = false;
};

// Helper function to load the reference coordinate values trajectory and
// validate the model. The coordinate values table is modified to update the
// column labels based on the model coordinate paths, to update any
// coordinates dependent on `CoordinateCouplerConstraint`s, and to convert
// the coordinate values to radians if the "inDegrees" metadata flag is set
// to "yes".
TimeSeriesTable loadCoordinateValuesAndValidateModel(
        const std::string& documentDir,
        TableProcessor tableProcessor,
        Model& model) {

    const auto pathList = model.getComponentList<AbstractGeometryPath>();
    int numPaths = (int)std::distance(pathList.begin(), pathList.end());
    OPENSIM_THROW_IF(!numPaths, Exception,
            "Expected the model to contain at least one AbstractGeometryPath, "
            "but it does not.")

    const auto fbPathList = model.getComponentList<FunctionBasedPath>();
    int numFunctionBasedPaths = (int)std::distance(fbPathList.begin(),
            fbPathList.end());
    OPENSIM_THROW_IF(numFunctionBasedPaths, Exception,
            "Expected the model to not contain any FunctionBasedPaths, but it "
            "does. Please remove all FunctionBasedPaths from the model before "
            "running the PolynomialPathFitter.")

    // Load the coordinate values.
    tableProcessor.append(TabOpConvertDegreesToRadians());
    tableProcessor.append(TabOpUseAbsoluteStateNames());
    tableProcessor.append(TabOpAppendCoupledCoordinateValues());
    TimeSeriesTable values = tableProcessor.process(documentDir, &model);
    log_info("Coordinate values table: {} columns, {} time points",
            values.getNumColumns(), values.getNumRows());

    // Remove any rows in the coordinate values table provided by the user that
    // contain NaN values.
    const auto& times = values.getIndependentColumn();
    for (int i = 0; i < static_cast<int>(times.size()); ++i) {
        if (SimTK::isNaN(values.getRowAtIndex(i).sum())) {
            values.removeRow(times[i]);
            log_info("NaN coordinate value detected at time {:1.2f} s, "
                     "removing row...", times[i]);
        }
    }

    // Validate the coordinate values table
    std::vector<std::string> jointsToWeld;
    for (auto& coordinate : model.updComponentList<Coordinate>()) {
        std::string valuePath = fmt::format("{}/value",
                coordinate.getAbsolutePathString());

        // If the coordinate is locked, but the user provided a column for the
        // coordinate, then we unlock it. Otherwise, we will weld the joint that
        // the coordinate belongs to.
        if (coordinate.get_locked()) {
            if (values.hasColumn(valuePath)) {
                coordinate.set_locked(false);
            } else {
                jointsToWeld.push_back(coordinate.getJoint().getName());
            }
        } else {
            OPENSIM_THROW_IF(!values.hasColumn(valuePath), Exception,
                    fmt::format("Expected the coordinate values table to "
                                "contain a column for '{}' (this coordinate is "
                                "not locked), but it does not.",
                            coordinate.getAbsolutePathString()))
        }
    }

    // If we detected any joints to be welded, update the model.
    if (!jointsToWeld.empty()) {
        log_info("Welding the following locked joints (no data provided): ");
        for (const auto& jointName : jointsToWeld) {
            log_info("  {}", jointName);
        }
        auto modelProcessor = ModelProcessor(model) |
                              ModOpReplaceJointsWithWelds(jointsToWeld);
        model = modelProcessor.process();
    }

    // Delete any columns in the coordinate values table that are not in the
    // model.
    std::vector<std::string> columnsToDelete;
    for (const auto& columnLabel : values.getColumnLabels()) {
        // Check if the column label ends with "/value".
        if (columnLabel.substr(columnLabel.size() - 6) != "/value") {
            values.removeColumn(columnLabel);
            continue;
        }
        // Check if the column label contains a coordinate path.
        const std::string coordinatePath = columnLabel.substr(
                0, columnLabel.size() - std::string("/value").size());
        if (!model.hasComponent(coordinatePath)) {
            values.removeColumn(columnLabel);
        }
    }

    return values;
}

// Helper function to sample coordinate values around the user-provided
// coordinate trajectory contained in the `values` input table. The
// sampling is defined based on the coordinate bounds and range maps,
// the number of samples per frame, and the Latin hypercube sampling
// algorithm.
TimeSeriesTable sampleCoordinateValues(
        const TimeSeriesTable& values,
        const CoordinateMap& coordinateBoundsMap,
        const CoordinateMap& coordinateRangeMap,
        const Settings& settings) {

    // Mute the Latin hypercube sampling output, so it doesn't print out for
    // every time point.
    Logger::Level origLoggerLevel = Logger::getLevel();
    Logger::setLevel(Logger::Level::Warn);

    // Create a Latin hypercube design to sample the coordinate values.
    LatinHypercubeDesign lhs;
    lhs.setNumSamples(settings.numSamplesPerFrame);
    lhs.setNumVariables((int)values.getNumColumns());

    // Helper function for sampling the coordinate values between two time
    // indexes.
    auto sampleCoordinateValuesSubset = [lhs](
            std::vector<int>::iterator begin_iter,
            std::vector<int>::iterator end_iter,
            const TimeSeriesTable& values,
            const CoordinateMap& coordinateBoundsMap,
            const CoordinateMap& coordinateRangeMap,
            bool useStochasticEvolutionaryLHS) -> SimTK::Matrix {

        SimTK::Matrix results(
                lhs.getNumSamples()*(int)std::distance(begin_iter, end_iter),
                (int)values.getNumColumns());
        SimTK::Matrix design(lhs.getNumSamples(), lhs.getNumVariables());
        int thisTimeIndex = 0;
        for (auto it = begin_iter; it != end_iter; ++it) {
            // Generate the design and shift its values between [-1, 1].
            if (useStochasticEvolutionaryLHS) {
                design = lhs.generateStochasticEvolutionaryDesign();
            } else {
                design = lhs.generateRandomDesign();
            }
            design.elementwiseSubtractFromScalarInPlace(0.5);
            design *= 2;

            int icol = 0;
            for (const std::string& label : values.getColumnLabels()) {
                const SimTK::VectorView column =
                        values.getDependentColumn(label);
                const auto& bounds = coordinateBoundsMap.at(label);
                const auto& range = coordinateRangeMap.at(label);

                // Linearly transform the design to the specified bounds for
                // each coordinate.
                double slope = 0.5*(bounds[1] - bounds[0]);
                SimTK::Vector candidateCol = slope*(design.col(icol) + 1.0) +
                        bounds[0] + column[*it];

                // Check that all elements in the column are within the range of
                // motion. If not, move the element inside the range of motion
                // that is closest to the original value.
                for (double& elt : candidateCol) {
                    if (elt < range[0]) {
                        elt = range[0];
                    } else if (elt > range[1]) {
                        elt = range[1];
                    }
                }
                design.updCol(icol) = candidateCol;
                ++icol;
            }

            // Store the results.
            results.updBlock(thisTimeIndex*lhs.getNumSamples(), 0,
                    lhs.getNumSamples(), (int)values.getNumColumns()) = design;
            ++thisTimeIndex;
        }

        return results;
    };

    // Determine the number of threads and the stride based on the number of
    // forces and the number of threads.
    int numTimePoints = static_cast<int>(values.getNumRows());
    int numThreads = std::min(numTimePoints, settings.numParallelThreads);
    int stride = static_cast<int>(std::floor(numTimePoints / numThreads));
    int offset = 0;

    // Divide the sampling across multiple threads.
    std::vector<int> timeIndexes(values.getNumRows());
    std::iota(timeIndexes.begin(), timeIndexes.end(), 0);
    std::vector<std::future<SimTK::Matrix>> futures;
    for (int thread = 0; thread < numThreads; ++thread) {
        auto begin_iter = timeIndexes.begin() + offset;
        auto end_iter = (thread == numThreads-1) ?
                timeIndexes.end() :
                timeIndexes.begin() + offset + stride;
        futures.push_back(std::async(std::launch::async,
                sampleCoordinateValuesSubset,
                begin_iter, end_iter, values, coordinateBoundsMap,
                coordinateRangeMap, settings.useStochasticEvolutionaryLHS));
        offset += stride;
    }

    // Wait for threads to finish and collect the results.
    std::vector<SimTK::Matrix> outputs;
    outputs.reserve(numThreads);
    for (int i = 0; i < numThreads; ++i) {
        outputs.push_back(futures[i].get());
    }

    // Reset the logger.
    OpenSim::Logger::setLevel(origLoggerLevel);

    // Assemble the results into one TimeSeriesTable.
    int timeIdx = 0;
    const auto& times = values.getIndependentColumn();
    TimeSeriesTable valuesSampled;
    double dt = (times.size() < 2) ? 0.01 :
                (times[1] - times[0]) / (settings.numSamplesPerFrame + 2);
    for (int i = 0; i < numThreads; ++i) {
        int numTimeIndexes = outputs[i].nrow() / settings.numSamplesPerFrame;
        for (int j = 0; j < numTimeIndexes; ++j) {
            // Append the original values.
            valuesSampled.appendRow(times[timeIdx],
                    values.getRowAtIndex(timeIdx));

            // Update the time step, if possible. Otherwise, use the last time
            // step.
            if (timeIdx+1 < static_cast<int>(values.getNumRows())) {
                dt = (times[timeIdx+1] - times[timeIdx]) /
                     (settings.numSamplesPerFrame + 2);
            }

            // Append the sampled values.
            for (int irow = 0; irow < settings.numSamplesPerFrame; ++irow) {
                valuesSampled.appendRow(times[timeIdx] + (irow + 1)*dt,
                        outputs[i].row(irow + j*settings.numSamplesPerFrame));
            }
            ++timeIdx;
        }
    }
    valuesSampled.addTableMetaData<std::string>("inDegrees", "no");
    valuesSampled.setColumnLabels(values.getColumnLabels());

    return valuesSampled;
}

// Helper function to compute path lengths and moment arms for the
// geometry-based paths in the model. The path lengths and moment arms
// are computed using the coordinate values in the `coordinateValues`
// table. The `numThreads` argument specifies the number of threads used
// to parallelize the computations.
void computePathLengthsAndMomentArms(
        const Model& model,
        const TimeSeriesTable& coordinateValues,
        const std::vector<std::string>& forcePaths,
        const MomentArmMap& momentArmMap,
        const Settings& settings,
        TimeSeriesTable& pathLengths,
        TimeSeriesTable& momentArms) {

    // Precalculate some useful values for the path length and moment arm
    // computations.
    const int numTimes = static_cast<int>(coordinateValues.getNumRows());
    OPENSIM_THROW_IF(numTimes == 0, Exception,
            "Tried to compute path lengths and moment arms, but the provided "
            "coordinate values table is empty.");
    const int numPaths = static_cast<int>(forcePaths.size());
    const int numThreads = std::min(numPaths, settings.numParallelThreads);
    int numMomentArms = 0;
    for (const auto& [forcePath, coordinatePaths] : momentArmMap) {
        numMomentArms += coordinatePaths.size();
    }

    // Define the path calculation worker. The atomic integer 'nextPathIndex'
    // controls the path from a thread should perform length and moment arm
    //  calculations.
    std::atomic<int> nextPathIndex(0);
    std::vector<SimTK::Matrix> pathResults(numPaths);
    auto pathCalculationWorker = [&](Model model, StatesTrajectory statesTraj) {
        model.initSystem();

        int ipath;
        while ((ipath = nextPathIndex.fetch_add(1)) < numPaths) {
            const std::string& forcePath = forcePaths[ipath];
            log_info("Computing values for path {} of {}: '{}'...",
                     ipath+1, numPaths, forcePath);

            // Resolve the path and coordinate components once, before
            // iterating over the states.
            const auto& force = model.getComponent<Force>(forcePath);
            const AbstractGeometryPath& path =
                    force.getPropertyByName<AbstractGeometryPath>("path")
                            .getValue();
            std::vector<SimTK::ReferencePtr<const Coordinate>> coordinates;
            for (const auto& coordinatePath : momentArmMap.at(forcePath)) {
                coordinates.emplace_back(
                        &model.getComponent<Coordinate>(coordinatePath));
            }
            const int numCoordsThisPath = static_cast<int>(coordinates.size());

            // The result matrix for this path contains the path length
            // column followed by the moment arm columns.
            SimTK::Matrix& results = pathResults[ipath];
            results.resize(numTimes, 1 + numCoordsThisPath);
            for (int irow = 0; irow < numTimes; ++irow) {
                const auto& state = statesTraj.get(irow);

                // Attempt to compute the path length and moment arms for the
                // current state. If an error occurs, set the values to NaN.
                // This sample will be filtered out later on before fitting
                // the polynomial coefficients.
                try {
                    model.realizePosition(state);

                    // Compute path length.
                    results(irow, 0) = path.getLength(state);

                    // Compute moment arms.
                    for (int ic = 0; ic < numCoordsThisPath; ++ic) {
                        results(irow, 1 + ic) =
                                path.computeMomentArm(state, *coordinates[ic]);
                    }
                } catch (const std::exception& e) {
                    log_warn("Skipping sample at time {:1.2f} s for path "
                             "'{}' due to error in path computation: {}",
                            state.getTime(), forcePath, e.what());
                    // Inject NaN values in the row associated with this
                    // sample so that it is filtered out later on before
                    // fitting the polynomial coefficients.
                    results.updRow(irow).setToNaN();
                }
            }
        }
    };

    // Construct a StatesTrajectory from the coordinates table.
    auto statesTraj = StatesTrajectory::createFromStatesTable(
            model, coordinateValues, true, false, false);
    OPENSIM_ASSERT_ALWAYS(numTimes == static_cast<int>(statesTraj.getSize()));

    // Launch the worker threads.
    std::vector<std::future<void>> futures;
    futures.reserve(numThreads);
    for (int thread = 0; thread < numThreads; ++thread) {
        futures.push_back(std::async(std::launch::async,
                pathCalculationWorker, model, statesTraj));
    }

    // Wait for all threads to finish.
    for (auto& future : futures) {
        future.get();
    }

    // Assemble the per-path results column-wise into matrices of path
    // lengths and moment arms.
    SimTK::Matrix lengthsMatrix(numTimes, numPaths);
    SimTK::Matrix momentArmsMatrix(numTimes, numMomentArms);
    int ipath = 0;
    int offset = 0;
    for (const auto& path : forcePaths) {
        const SimTK::Matrix& results = pathResults[ipath];
        const int numCoords = results.ncol() - 1;
        lengthsMatrix.updCol(ipath) = results.col(0);
        momentArmsMatrix.updBlock(0, offset, numTimes,
                numCoords) = results.block(0, 1, numTimes, numCoords);
        ++ipath;
        offset += numCoords;
    }

    // Assemble the results into TimeSeriesTables.
    std::vector<std::string> pathLengthLabels;
    pathLengthLabels.reserve(numPaths);
    std::vector<std::string> momentArmLabels;
    momentArmLabels.reserve(numMomentArms);
    for (const auto& forcePath : forcePaths) {
        pathLengthLabels.push_back(fmt::format("{}_length", forcePath));
        for (const auto& coordinatePath : momentArmMap.at(forcePath)) {
            momentArmLabels.push_back(fmt::format("{}_moment_arm_{}",
                    forcePath,
                    ComponentPath(coordinatePath).getComponentName()));
        }
    }
    const std::vector<double>& times = coordinateValues.getIndependentColumn();
    pathLengths = TimeSeriesTable(times, lengthsMatrix, pathLengthLabels);
    momentArms = TimeSeriesTable(times, momentArmsMatrix, momentArmLabels);
}

// Helper function to filter out bad coordinate value samples. Bad samples
// are defined as coordinate values that produce path length and/or moment
// arm values that contain NaNs or deviate by a set number of standard
// deviations away from the nominal trajectories.
void filterSampledData(
        const Settings& settings,
        TimeSeriesTable& coordinateValues,
        TimeSeriesTable& pathLengths,
        TimeSeriesTable& momentArms) {

    // Remove any path length or moment arm rows that contain NaN values.
    std::vector<double> times = coordinateValues.getIndependentColumn();
    for (const auto& time : times) {
        bool momentArmNaN = SimTK::isNaN(momentArms.getRow(time).sum());
        bool pathLengthNaN = SimTK::isNaN(pathLengths.getRow(time).sum());
        if (momentArmNaN || pathLengthNaN) {
            pathLengths.removeRow(time);
            momentArms.removeRow(time);
            coordinateValues.removeRow(time);
            log_info("NaN path length or moment arm detected at time {:1.2f} s, "
                     "removing row...", time);
        }
    }

    // Remove sample points that fall outside two standard deviations of the
    // data in each column.
    double threshold = 5.0;
    std::vector<double> rejectedTimePoints;
    auto rejectTimePoints = [threshold, &settings](
            const TimeSeriesTable& table,
            std::vector<double>& rejectedTimePoints) {
        const auto& times = table.getIndependentColumn();
        int increment = settings.numSamplesPerFrame + 1;
        int numOriginalTimes = (int)table.getNumRows() / increment;
        for (const auto& label : table.getColumnLabels()) {
            SimTK::Vector column = table.getDependentColumn(label);

            SimTK::Vector std(numOriginalTimes, 0.0);
            for (int i = 0; i < numOriginalTimes; ++i) {
                // Compute the mean and standard deviation segments of the
                // column segment.
                SimTK::Vector segment = column.block(i*increment, 0,
                        increment, 1).getAsVector();
                double mean = segment.sum() / segment.size();
                segment.elementwiseSubtractFromScalarInPlace(mean);
                std[i] = std::sqrt(segment.normSqr() / segment.size());
            }

            // Compute the average standard deviation.
            double avgStd = std.sum() / std.size();

            // Find the time points that deviate too far from the nominal
            // values.
            int currentNominalIndex = 0;
            for (int i = 0; i < column.size(); ++i) {
                double nominal = column[currentNominalIndex];
                if (std::abs(column[i] - nominal) > threshold * avgStd) {
                    rejectedTimePoints.push_back(times[i]);
                }
                if ((i + 1) % increment == 0 &&
                        currentNominalIndex + increment < column.size()) {
                    currentNominalIndex += increment;
                }
            }
        }
    };
    rejectTimePoints(pathLengths, rejectedTimePoints);
    rejectTimePoints(momentArms, rejectedTimePoints);

    // Remove duplicate time points.
    std::sort(rejectedTimePoints.begin(), rejectedTimePoints.end());
    rejectedTimePoints.erase(std::unique(rejectedTimePoints.begin(),
            rejectedTimePoints.end()), rejectedTimePoints.end());

    // Remove the rejected time points.
    if (!rejectedTimePoints.empty()) {
        double percentTotal = 100.0 * (int)rejectedTimePoints.size() /
                                      (int)coordinateValues.getNumRows();
        log_info("Removing {} samples ({:1.1f}% of total) that are larger than "
                 "{} standard deviations from nominal values...",
                rejectedTimePoints.size(), percentTotal, threshold);
        for (double time : rejectedTimePoints) {
            coordinateValues.removeRow(time);
            pathLengths.removeRow(time);
            momentArms.removeRow(time);
        }
    }
}

// Fit to the path length and moment arm samples using all possible
// polynomial coefficients. `coordinates` is the matrix of coordinate values
// for coordinates that the current path depends on. The vector `b` contains
// the path length and moment arm values for the current path.
// We solve for the coefficients of the polynomial using a least squares of
// fit, `Ax = b`. Each row of `A` contains polynomial terms evaluated using
// the coordinate values from a particular point in time. `x` is the vector
// polynomial coefficients. The first N elements of `b` contain the path
// length values, where N is the number of time points. The remaining N*Nc
// rows of `b` contain the moment arm values, where Nc is the number of
// coordinates the path depends on.
int fitAllCoefficients(
        const SimTK::Matrix& coordinates,
        const SimTK::Vector& b,
        const Settings& settings,
        SimTK::Vector& coefficients) {

    int numTimes = coordinates.nrow();
    int numCoordinates = coordinates.ncol();
    int order = settings.minimumPolynomialOrder;
    while (true) {
        // Initialize the multivariate polynomial function.
        int numCoefficients = choose(numCoordinates + order, order);
        SimTK::Vector dummyCoefficients(numCoefficients, 1.0);
        MultivariatePolynomialFunction dummyFunction(dummyCoefficients,
                numCoordinates, order);

        // Initialize the 'A' matrix.
        SimTK::Matrix A(numTimes * (numCoordinates + 1),
                numCoefficients, 0.0);

        // Fill in the A matrix. This contains the polynomial terms for
        // the path length and moment arms.
        for (int itime = 0; itime < numTimes; ++itime) {
            A(itime, 0, 1, numCoefficients) =
                    dummyFunction.getTermValues(
                        coordinates.row(itime).transpose().getAsVector());

            for (int ic = 0; ic < numCoordinates; ++ic) {
                SimTK::Vector termDerivatives =
                    dummyFunction.getTermDerivatives({ic},
                        coordinates.row(itime).transpose().getAsVector())
                        .negate();
                A((ic+1)*numTimes + itime, 0, 1, numCoefficients) =
                        termDerivatives;
            }
        }

        // Solve the least-squares problem.
        SimTK::FactorQTZ factor(A);
        factor.solve(b, coefficients);

        // Calculate the RMS error.
        SimTK::Vector b_fit = A * coefficients;
        SimTK::Vector error = b - b_fit;

        // If the fit achieves the path length and moment arm thresholds
        // we set, then exit the loop.
        SimTK::Vector pathLengthError = error.block(
                0, 0, numTimes, 1).getAsVector();
        double pathLengthRMSError = std::sqrt(
                pathLengthError.normSqr() / pathLengthError.size());
        bool pathLengthMet = pathLengthRMSError < settings.pathLengthTolerance;

        bool momentArmMet = true;
        for (int ic = 0; ic < numCoordinates; ++ic) {
            SimTK::Vector momentArmError = error.block(
                    numTimes + ic*numTimes, 0, numTimes, 1).getAsVector();
            double momentArmRMSError = std::sqrt(
                    momentArmError.normSqr() / numTimes);
            if (momentArmRMSError > settings.momentArmTolerance) {
                momentArmMet = false;
                break;
            }
        }

        if ((pathLengthMet && momentArmMet) ||
                order == settings.maximumPolynomialOrder) {
            break;
        }
        ++order;
    }

    return order;
}

// Fit to the path length and moment arm samples using stepwise regression
// to find a minimal set of polynomial coefficients. `coordinates` is the
// matrix of coordinate values for coordinates that the current path depends
// on. The vector `b` contains the path length and moment arm values for the
// current path.
void fitCoefficientsStepwiseRegression(
        const SimTK::Matrix& coordinates,
        const SimTK::Vector& b,
        const Settings& settings,
        SimTK::Vector& coefficients) {

    // Preliminaries.
    int numTimes = coordinates.nrow();
    int numCoordinates = coordinates.ncol();
    int order = settings.maximumPolynomialOrder;
    int numCoefficients = choose(numCoordinates + order, order);

    // Construct the full 'A' matrix.
    SimTK::Vector dummyCoefficients(numCoefficients, 1.0);
    MultivariatePolynomialFunction dummyFunction(dummyCoefficients,
            numCoordinates, order);
    SimTK::Matrix Afull(numTimes * (numCoordinates + 1),
            numCoefficients, 0.0);
    for (int itime = 0; itime < numTimes; ++itime) {
        Afull(itime, 0, 1, numCoefficients) =
                dummyFunction.getTermValues(
                    coordinates.row(itime).transpose().getAsVector());

        for (int ic = 0; ic < numCoordinates; ++ic) {
            SimTK::Vector termDerivatives =
                dummyFunction.getTermDerivatives({ic},
                    coordinates.row(itime).transpose().getAsVector())
                    .negate();
            Afull((ic+1)*numTimes + itime, 0, 1, numCoefficients) =
                    termDerivatives;
        }
    }

    // Manage the coefficient indexes that will be included in the final
    // polynomial. The "out" indexes are the indexes that are not included in
    // the final polynomial, which is initialized to all indexes. The "keep"
    // indexes are the indexes that are included in the final polynomial, which
    // is initialized to an empty vector.
    std::vector<int> outIndexes;
    outIndexes.reserve(numCoefficients);
    for (int i = 0; i < numCoefficients; ++i) {
        outIndexes.push_back(i);
    }
    std::vector<int> keepIndexes;
    SimTK::Vector x_sol;
    while (true) {

        // Initialize the 'A' matrix.
        // The number of terms in the polynomial is the number of "kept"
        // coefficients plus one.
        int numTerms = static_cast<int>(keepIndexes.size()) + 1;
        SimTK::Matrix A(numTimes * (numCoordinates + 1), numTerms, 0.0);
        SimTK::Vector x(numTerms, 0.0);

        // First, set the columns of 'A' for the "kept" coefficients.
        int icol = 0;
        for (const auto& ki : keepIndexes) {
            A.updCol(icol++) = Afull.col(ki);
        }

        // Next, loop through all of the remaining "out" coefficients and fit
        // the polynomial. We will keep the coefficient that results in the
        // smallest RMS error.
        SimTK::Real bestError = SimTK::Infinity;
        int bestIndex = -1;
        for (const auto& oi : outIndexes) {
            A.updCol(icol) = Afull.col(oi);

            // Solve the least-squares problem.
            SimTK::FactorQTZ factor(A);
            factor.solve(b, x);

            // Calculate the RMS error.
            SimTK::Vector b_fit = A * x;
            SimTK::Vector error = b - b_fit;

            // Calculate the RMS error. Update the best error and index.
            double currentError = std::sqrt(error.normSqr() / error.size());
            if (currentError < bestError) {
                bestError = currentError;
                bestIndex = oi;
            }
        }

        // Add the best index to the "keep" indexes and remove it from the "out"
        // indexes.
        keepIndexes.push_back(bestIndex);
        outIndexes.erase(std::remove(outIndexes.begin(), outIndexes.end(),
                bestIndex), outIndexes.end());

        // Refit the polynomial with the "keep" indexes.
        icol = 0;
        for (const auto& ki : keepIndexes) {
            A.updCol(icol++) = Afull.col(ki);
        }
        SimTK::FactorQTZ factor(A);
        factor.solve(b, x);
        SimTK::Vector b_fit = A * x;
        SimTK::Vector error = b - b_fit;

        // If the current polynomial achieves our path length and moment arm
        // tolerances or if the "out" indexes is empty, exit the loop.
        SimTK::Vector pathLengthError = error.block(
                0, 0, numTimes, 1).getAsVector();
        double pathLengthRMSError = std::sqrt(
                pathLengthError.normSqr() / pathLengthError.size());
        bool pathLengthMet = pathLengthRMSError < settings.pathLengthTolerance;

        bool momentArmMet = true;
        for (int ic = 0; ic < numCoordinates; ++ic) {
            SimTK::Vector momentArmError = error.block(
                    numTimes + ic*numTimes, 0, numTimes, 1).getAsVector();
            double momentArmRMSError = std::sqrt(
                    momentArmError.normSqr() / numTimes);
            if (momentArmRMSError > settings.momentArmTolerance) {
                momentArmMet = false;
                break;
            }
        }

        int numOutIndexes = static_cast<int>(outIndexes.size());
        if ((pathLengthMet && momentArmMet) || !numOutIndexes) {
            x_sol = x;
            break;
        }
    }

    // Update the coefficients vector
    coefficients.resize(numCoefficients);
    coefficients.setToZero();
    int icoeff = 0;
    for (const auto& ki : keepIndexes) {
        coefficients.set(ki, x_sol(icoeff++));
    }
}

// Helper function to fit polynomial coefficients to the path lengths and
// moment arms computed from the geometry-based paths in the model. The
// `coordinateValues`, `pathLengths`, and `momentArms` table arguments are
// the result of previous model sampling and data filtering steps. The
// `momentArmMap` argument is a map containing the coordinates each path is
// dependent on, which determines the number of independent coordinates
// that each `MultivariatePolynomialFunction` contains to approximate the
// original path.
Set<FunctionBasedPath> fitPolynomialCoefficients(
        const TimeSeriesTable& coordinateValues,
        const std::vector<std::string>& forcePaths,
        const TimeSeriesTable& pathLengths,
        const TimeSeriesTable& momentArms,
        const MomentArmMap& momentArmMap,
        const Settings& settings) {

    const int numTimes = (int)coordinateValues.getNumRows();

    // Pre-compute variables.
    // ----------------------
    const int numForces = (int)forcePaths.size();
    // Force indexes: one per path-based force (indices into forcePaths).
    std::vector<int> forceIndexes;
    forceIndexes.reserve(numForces);
    for (int i = 0; i < numForces; ++i) {
        forceIndexes.push_back(i);
    }

    // Build a FunctionBasedPath for each path-based force in the model.
    // -----------------------------------------------------------------
    // Solve A*x = b, where x is the vector of coefficients for the
    // FunctionBasedPath, A is a matrix of polynomial terms, and b is a vector
    // of path lengths and moment arms.
    auto fitForcePolynomialSubset = [&](
            std::vector<int>::iterator begin_iter,
            std::vector<int>::iterator end_iter,
            int thread) -> std::vector<std::unique_ptr<FunctionBasedPath>> {

        std::vector<std::unique_ptr<FunctionBasedPath>> thesePaths;
        thesePaths.reserve(std::distance(begin_iter, end_iter));
        for (auto it = begin_iter; it != end_iter; ++it) {
            int iforce = *it;
            const std::string& forcePath = forcePaths[iforce];

            // Check if the current force is dependent on any coordinates in the
            // model. If not, skip it.
            if (momentArmMap.find(forcePath) == momentArmMap.end()) {
                continue;
            }
            log_info("Thread {:2d}/{:2d}: fitting coefficients for force "
                     "'{}'...", thread+1, settings.numParallelThreads,
                    forcePath);

            // The coordinate paths that the current force depends on.
            const std::vector<std::string>& coordinatePathsThisForce =
                    momentArmMap.at(forcePath);
            int numCoordinatesThisForce = (int)coordinatePathsThisForce.size();

            // Initialize the 'b' vector. This is the same for all polynomial
            // orders.
            SimTK::Vector b(numTimes * (numCoordinatesThisForce + 1), 0.0);

            // The path lengths for this force. This is the first N elements of
            // the 'b' vector.
            b(0, numTimes) = pathLengths.getDependentColumn(
                    fmt::format("{}_length", forcePath));

            // The moment arms this force and coordinates associated with this
            // force. The moment arms are the remaining elements of the 'b'
            // vector.
            SimTK::Matrix coordinatesThisForce(
                    numTimes, numCoordinatesThisForce, 0.0);
            for (int ic = 0; ic < numCoordinatesThisForce; ++ic) {
                const std::string coordinateName = ComponentPath(
                        coordinatePathsThisForce[ic]).getComponentName();
                b((ic+1)*numTimes, numTimes) = momentArms.getDependentColumn(
                        fmt::format("{}_moment_arm_{}", forcePath,
                                coordinateName));

                const SimTK::VectorView coordinateValuesThisCoordinate =
                        coordinateValues.getDependentColumn(
                            fmt::format("{}/value",
                                        coordinatePathsThisForce[ic]));
                for (int itime = 0; itime < numTimes; ++itime) {
                    coordinatesThisForce.set(
                            itime, ic, coordinateValuesThisCoordinate[itime]);
                }
            }

            // Polynomial fitting.
            // -------------------
            SimTK::Vector coefficients;
            int order;
            if (settings.useStepwiseRegression) {
                order = settings.maximumPolynomialOrder;
                fitCoefficientsStepwiseRegression(coordinatesThisForce, b,
                        settings, coefficients);
            } else {
                order = fitAllCoefficients(coordinatesThisForce, b,
                        settings, coefficients);
            }

            // Create a FunctionBasedPath for the current path-based force.
            MultivariatePolynomialFunction lengthFunction;
            lengthFunction.setDimension(numCoordinatesThisForce);
            lengthFunction.setOrder(order);
            lengthFunction.setCoefficients(coefficients);
            auto functionBasedPath = std::make_unique<FunctionBasedPath>();
            functionBasedPath->setName(forcePath);
            functionBasedPath->setCoordinatePaths(coordinatePathsThisForce);
            functionBasedPath->setLengthFunction(lengthFunction);
            if (settings.includeMomentArmFunctions) {
                for (int iq = 0; iq < numCoordinatesThisForce; ++iq) {
                    MultivariatePolynomialFunction momentArmFunction =
                            lengthFunction.generateDerivativeFunction(iq, true);
                    functionBasedPath->appendMomentArmFunction(
                            momentArmFunction);
                }
            }
            if (settings.includeLengtheningSpeedFunction) {
                MultivariatePolynomialFunction lengtheningSpeedFunction =
                        lengthFunction.generatePartialVelocityFunction();
                functionBasedPath->setLengtheningSpeedFunction(
                        lengtheningSpeedFunction);
            }

            // Save the FunctionBasedPath.
            thesePaths.push_back(std::move(functionBasedPath));
        }

        return thesePaths;
    };

    // Determine the number of threads and the stride based on the number of
    // forces and the number of threads.
    int numThreads = std::min(numForces, settings.numParallelThreads);
    int stride = static_cast<int>(std::floor(numForces / numThreads));
    int offset = 0;

    // Divide the polynomial fitting across multiple threads.
    std::vector<std::future<std::vector<std::unique_ptr<FunctionBasedPath>>>>
            futures;
    for (int thread = 0; thread < numThreads; ++thread) {
        auto begin_iter = forceIndexes.begin() + offset;
        auto end_iter = (thread == numThreads-1) ?
                forceIndexes.end() :
                forceIndexes.begin() + offset + stride;
        futures.push_back(std::async(std::launch::async,
                        fitForcePolynomialSubset,
                        begin_iter, end_iter, thread));
        offset += stride;
    }

    // Wait for threads to finish and collect the results.
    Set<FunctionBasedPath> functionBasedPaths;
    for (int thread = 0; thread < numThreads; ++thread) {
        auto thesePaths = futures[thread].get();
        for (auto& path : thesePaths) {
            functionBasedPaths.adoptAndAppend(path.release());
        }
    }

    return functionBasedPaths;
}

// Get the RMS errors between two sets of path lengths and moment arms
// computed from a model with FunctionBasedPaths and the original model. The
// `modelFitted` argument must be the model with the FunctionBasedPaths.
void computeFittingErrors(
        const Model& modelFitted,
        const TimeSeriesTable& pathLengths,
        const TimeSeriesTable& momentArms,
        const TimeSeriesTable& pathLengthsFitted,
        const TimeSeriesTable& momentArmsFitted,
        const Settings& settings) {

    // Convert the tolerances to centimeters.
    double pathLengthTolerance = settings.pathLengthTolerance * 100.0;
    double momentArmTolerance = settings.momentArmTolerance * 100.0;

    // Helper function for printing warning messages.
    auto printWarningMessage = [](const std::string& pathLengthOrMomentArm,
                                double tolerance) {
        log_warn("-----------------------------------------------------------");
        log_warn(fmt::format("The {} RMS error is greater than the prescribed "
                             "tolerance of {:g} cm.",
                             pathLengthOrMomentArm, tolerance));
        log_warn("");
        log_warn("Consider increasing the number of samples per frame or the ");
        log_warn("polynomial order and re-fitting the model. If a re-fitting ");
        log_warn("is not successful, check that the original model produces ");
        log_warn("path lengths and moment arms that are free of ");
        log_warn("discontinuities or other irregularities.");
        log_warn("-----------------------------------------------------------");
    };

    // Check inputs.
    OPENSIM_ASSERT_ALWAYS(pathLengths.getNumRows() ==
            pathLengthsFitted.getNumRows());
    OPENSIM_ASSERT_ALWAYS(momentArms.getNumRows() ==
            momentArmsFitted.getNumRows());
    OPENSIM_ASSERT_ALWAYS(pathLengths.getNumColumns() ==
            pathLengthsFitted.getNumColumns());
    OPENSIM_ASSERT_ALWAYS(momentArms.getNumColumns() ==
            momentArmsFitted.getNumColumns());
    const auto& functionBasedPaths =
            modelFitted.getComponentList<FunctionBasedPath>();
    int numPaths = (int)std::distance(functionBasedPaths.begin(),
            functionBasedPaths.end());
    OPENSIM_THROW_IF(numPaths == 0, Exception,
            "Expected the model to contain 'FunctionBasedPath's, but none were "
            "found.");

    // Print the path length and moment arm RMS errors for each path.
    log_info("");
    log_info("Summary of path length and moment arm RMS errors");
    log_info("------------------------------------------------");
    SimTK::Vector pathLengthRMSErrors((int)pathLengths.getNumColumns());
    SimTK::Vector momentArmRMSErrors((int)momentArms.getNumColumns());
    int ip = 0;
    int ima = 0;
    for (const auto& path : modelFitted.getComponentList<FunctionBasedPath>()) {
        std::string pathName = path.getAbsolutePathString();
        pathName = pathName.substr(0, pathName.length() - 5);
        log_info("'{}' path errors:", pathName);

        const std::string lengthLabel = fmt::format("{}_length", pathName);
        SimTK::Vector pathLength = pathLengths.getDependentColumn(lengthLabel);
        SimTK::Vector pathLengthFitted = pathLengthsFitted.getDependentColumn(
                lengthLabel);
        SimTK::Vector pathLengthError = pathLength - pathLengthFitted;
        double pathLengthRMSError = 100.0 * std::sqrt(
                pathLengthError.normSqr() / pathLengthError.size());
        pathLengthRMSErrors[ip++] = pathLengthRMSError;
        log_info(" - path length RMSE: {:1.3f} cm", pathLengthRMSError);

        if (pathLengthRMSError > 10.0*pathLengthTolerance) {
            printWarningMessage("path length", pathLengthTolerance);
        }

        for (const auto& coordinatePath : path.getCoordinatePaths()) {
            const auto& coordinate =
                    modelFitted.getComponent<Coordinate>(coordinatePath);
            const std::string& coordinateName = coordinate.getName();

            const std::string momentArmLabel = fmt::format(
                "{}_moment_arm_{}", pathName, coordinateName);
            SimTK::Vector momentArm = momentArms.getDependentColumn(
                    momentArmLabel);
            SimTK::Vector momentArmFitted = momentArmsFitted.getDependentColumn(
                    momentArmLabel);
            SimTK::Vector momentArmError = momentArm - momentArmFitted;
            double momentArmRMSError = 100.0 * std::sqrt(
                    momentArmError.normSqr() / momentArmError.size());
            momentArmRMSErrors[ima++] = momentArmRMSError;
            log_info(" - '{}' moment arm RMSE: {:1.3f} cm", coordinateName,
                    momentArmRMSError);

            if (momentArmRMSError > 10.0*momentArmTolerance) {
                printWarningMessage(
                        fmt::format("'{}' moment arm", coordinateName),
                        momentArmTolerance);
            }

        }
        log_info("");
    }

    // Print the average path length and moment arm RMS errors.
    double averagePathLengthError = pathLengthRMSErrors.sum() /
                                    pathLengthRMSErrors.size();
    double averageMomentArmError = momentArmRMSErrors.sum() /
                                   momentArmRMSErrors.size();
    log_info("Average path length RMSE = {:1.3f} cm", averagePathLengthError);
    if (averagePathLengthError > pathLengthTolerance) {
        printWarningMessage("average path length", pathLengthTolerance);
    }

    log_info("Average moment arm RMSE  = {:1.3f} cm", averageMomentArmError);
    if (averageMomentArmError > momentArmTolerance) {
        printWarningMessage("average moment arm", momentArmTolerance);
    }
}

}

//=============================================================================
// POLYNOMIAL PATH FITTER BOUNDS
//=============================================================================
PolynomialPathFitterBounds::PolynomialPathFitterBounds() : Object() {
    setAuthors("Nicholas Bianco");
    constructProperties();
}

PolynomialPathFitterBounds::PolynomialPathFitterBounds(
        const std::string& coordinatePath, const SimTK::Vec2& bounds) :
        PolynomialPathFitterBounds() {
    set_coordinate_path(coordinatePath);
    OPENSIM_THROW_IF_FRMOBJ(bounds[0] >= bounds[1], Exception,
            "Expected the lower bound to be less than the upper bound, but "
            "it is not.")
    set_bounds(bounds);
}

void PolynomialPathFitterBounds::constructProperties() {
    constructProperty_coordinate_path("");
    constructProperty_bounds({-10.0, 10.0});
}

//=============================================================================
// POLYNOMIAL PATH FITTER
//=============================================================================
PolynomialPathFitter::PolynomialPathFitter() : Object() {
    setAuthors("Nicholas Bianco");
    constructProperties();
}

PolynomialPathFitter::~PolynomialPathFitter() noexcept = default;

PolynomialPathFitter::PolynomialPathFitter(
        PolynomialPathFitter const&) = default;

PolynomialPathFitter& PolynomialPathFitter::operator=(
        const PolynomialPathFitter&) = default;

PolynomialPathFitter::PolynomialPathFitter(
        PolynomialPathFitter&& other) = default;

PolynomialPathFitter& PolynomialPathFitter::operator=(
        PolynomialPathFitter&& other) = default;

//=============================================================================
// PATH FITTING PIPELINE
//=============================================================================

void PolynomialPathFitter::run() {

    log_info("");
    log_info("====================");
    log_info("PolynomialPathFitter");
    log_info("====================");
    log_info("");

    log_info("Step 1/9: Load the model and coordinate values table.");
    log_info("-----------------------------------------------------");

    // Load the model.
    m_model = get_model().process(getDocumentDirectory());
    SimTK::State state = m_model.initSystem();

    // Load the coordinate values table.
    m_values = loadCoordinateValuesAndValidateModel(
            getDocumentDirectory(), get_coordinate_values(), m_model);
    OPENSIM_THROW_IF_FRMOBJ(!m_values.getNumRows(), Exception,
            "Expected the coordinate values table to contain at least one row, "
            "but it is empty.");

    // Get the path-based forces in the model and construct the moment arm
    // map based on the coordinates each path depends on.
    std::vector<std::string> forcePaths;
    MomentArmMap momentArmMap;
    const auto& forces = m_model.getComponentList<Force>();
    for (const auto& force : forces) {
        if (force.hasProperty("path")) {
            const std::string forcePath = force.getAbsolutePathString();
            const AbstractGeometryPath& path =
                    force.getPropertyByName<AbstractGeometryPath>("path")
                            .getValue();
            auto coordinatePaths = path.findIndependentCoordinatePaths(state);
            if (coordinatePaths.empty()) {
                log_warn("Found path-based force element '{}', but did not "
                         "find any coordinates associated with its path; it "
                         "will be excluded from path fitting.", forcePath);
                continue;
            }

            forcePaths.push_back(forcePath);
            momentArmMap[forcePath] = std::move(coordinatePaths);
        }
    }
    OPENSIM_THROW_IF_FRMOBJ(forcePaths.empty(), Exception,
            "Expected the model to contain at least one path-based force "
            "whose path depends on at least one coordinate, but none were "
            "found.");

    log_info("");
    log_info("Step 2/9: Set the coordinate bounds.");
    log_info("------------------------------------");
    // Set the global bounds for all coordinates.
    SimTK::Vec2 globalBounds = get_global_coordinate_sampling_bounds();
    log_info("Global bounds: [{}, {}] degrees.",
             globalBounds[0], globalBounds[1]);
    globalBounds[0] = SimTK::convertDegreesToRadians(globalBounds[0]);
    globalBounds[1] = SimTK::convertDegreesToRadians(globalBounds[1]);

    // Initialize coordinate bounds based on whether coordinates are clamped
    // in the model.
    CoordinateMap coordinateBoundsMap;
    CoordinateMap coordinateRangeMap;
    coordinateBoundsMap.reserve(m_model.getNumCoordinates());
    coordinateRangeMap.reserve(m_model.getNumCoordinates());
    for (const auto& coordinate : m_model.getComponentList<Coordinate>()) {
        std::string valuePath = fmt::format("{}/value",
                coordinate.getAbsolutePathString());
        coordinateBoundsMap.insert({valuePath, globalBounds});

        // If the coordinate is clamped, then we also store the allowable range
        // of motion.
        if (coordinate.get_clamped()) {
            SimTK::Vec2 range = {coordinate.getRangeMin(),
                    coordinate.getRangeMax()};
            coordinateRangeMap.insert({valuePath, range});
        } else {
            coordinateRangeMap.insert({valuePath,
                    {-SimTK::Infinity, SimTK::Infinity}});
        }
    }

    // Set any coordinate-specific bounds. This will overwrite the default
    // bounds.
    for (int i = 0; i < getProperty_coordinate_sampling_bounds().size(); ++i) {
        const auto& coordinatePath =
                get_coordinate_sampling_bounds(i).get_coordinate_path();
        OPENSIM_THROW_IF_FRMOBJ(
                !m_model.hasComponent<Coordinate>(coordinatePath),
                Exception, "Expected the model to contain the coordinate '{}', "
                           "but it does not.", coordinatePath)
        SimTK::Vec2 bounds = get_coordinate_sampling_bounds(i).get_bounds();
        log_info("Bounds for coordinate '{}': [{}, {}] degrees.",
                coordinatePath, bounds[0], bounds[1]);

        bounds[0] = SimTK::convertDegreesToRadians(bounds[0]);
        bounds[1] = SimTK::convertDegreesToRadians(bounds[1]);
        std::string valuePath = fmt::format("{}/value", coordinatePath);
        coordinateBoundsMap[valuePath] = bounds;
    }

    log_info("");
    log_info("Step 3/9: Verify the user-defined settings.");
    log_info("-------------------------------------------");
    validateProperties();

    Settings settings;
    settings.useStepwiseRegression = get_use_stepwise_regression();
    settings.momentArmTolerance = get_moment_arm_tolerance();
    settings.pathLengthTolerance = get_path_length_tolerance();
    settings.minimumPolynomialOrder = get_minimum_polynomial_order();
    settings.maximumPolynomialOrder = get_maximum_polynomial_order();
    settings.numParallelThreads = get_num_parallel_threads();
    settings.numSamplesPerFrame = get_num_samples_per_frame();
    settings.useStochasticEvolutionaryLHS =
            get_latin_hypercube_algorithm() == "ESEA";
    settings.includeMomentArmFunctions = get_include_moment_arm_functions();
    settings.includeLengtheningSpeedFunction =
            get_include_lengthening_speed_function();

    // Construct the output directory.
    std::string outputDir = get_output_directory();
    if (outputDir.empty()) {
        if (getDocumentDirectory().empty()) {
            outputDir = SimTK::Pathname::getCurrentWorkingDirectory();
        } else {
            outputDir = getDocumentDirectory();
        }
    } else {
        IO::makeDir(outputDir);
    }

    log_info("");
    log_info("Step 4/9: Sample coordinate values around the provided trajectory.");
    log_info("------------------------------------------------------------------");
    TimeSeriesTable valuesSampled = sampleCoordinateValues(m_values,
            coordinateBoundsMap, coordinateRangeMap, settings);

    // Recompute the coupled coordinate values.
    auto tableProcessorSampled = TableProcessor(valuesSampled) |
                                 TabOpAppendCoupledCoordinateValues();
    valuesSampled = tableProcessorSampled.process(&m_model);
    log_info("Total number of samples = {}", valuesSampled.getNumRows());

    log_info("");
    log_info("Step 5/9: Compute path lengths and moment arms.");
    log_info("-----------------------------------------------");
    log_info("Computing path lengths and moment arms for the original "
             "coordinate data...");
    TimeSeriesTable pathLengths;
    TimeSeriesTable momentArms;
    computePathLengthsAndMomentArms(m_model, m_values, forcePaths,
            momentArmMap, settings, pathLengths, momentArms);

    log_info("");
    log_info("Computing path lengths and moment arms for the sampled "
             "coordinate data...");
    TimeSeriesTable pathLengthsSampled;
    TimeSeriesTable momentArmsSampled;
    computePathLengthsAndMomentArms(m_model, valuesSampled, forcePaths,
            momentArmMap, settings, pathLengthsSampled, momentArmsSampled);

    log_info("");
    log_info("Step 6/9: Filter the sampled path data.");
    log_info("---------------------------------------");
    filterSampledData(settings, valuesSampled, pathLengthsSampled,
            momentArmsSampled);

    log_info("");
    log_info("Step 7/9: Fit the polynomial coefficients.");
    log_info("------------------------------------------");
    Set<FunctionBasedPath> functionBasedPaths = fitPolynomialCoefficients(
            valuesSampled, forcePaths, pathLengthsSampled,
            momentArmsSampled, momentArmMap, settings);
    Array<std::string> pathNames;
    functionBasedPaths.getNames(pathNames);

    log_info("");
    log_info("Step 8/9: Evaluate the fit.");
    log_info("---------------------------");

    // Find the longest path name.
    int longestPathName = 0;
    for (int i = 0; i < pathNames.getSize(); ++i) {
        std::string pathName = pathNames[i];
        if ((int)pathName.size() > longestPathName) {
            longestPathName = (int)pathName.size();
        }
    }

    // Print the information for each path.
    log_info("");
    std::string pathName = fmt::format("{}path",
            std::string((int)(0.5*longestPathName)-2, ' '));
    std::string fitName = fmt::format("{}polynomial fit", std::string(15, ' '));
    std::string line = fmt::format("{:{}} | {:{}}", pathName, longestPathName,
            fitName, 44);
    std::string separator(line.size(), '-');
    log_info(separator);
    log_info(line);
    log_info(separator);
    for (int i = 0; i < functionBasedPaths.getSize(); ++i) {
        auto path = functionBasedPaths.get(pathNames[i]);
        auto function = dynamic_cast<const MultivariatePolynomialFunction&>(
                path.getLengthFunction());
        SimTK::Vector coefficients = function.getCoefficients();
        int numNonZeroCoeffs = 0;
        for (int i = 0; i < coefficients.size(); ++i) {
            if (coefficients[i] != 0.0) {
                ++numNonZeroCoeffs;
            }
        }
        line = fmt::format("{:{}} | order = {}, dimension = "
                "{}, coefficients = {}", path.getName(), longestPathName,
                function.getOrder(), function.getDimension(), numNonZeroCoeffs);
        log_info(line);
    }
    log_info(separator);

    // Add the FunctionBasedPaths to the model.
    log_info("");
    log_info("Computing path lengths and moment arms from the fitted paths...");
    Model modelFitted = m_model;
    modelFitted.initSystem();
    ModelFactory::replacePathsWithFunctionBasedPaths(modelFitted,
            functionBasedPaths);

    // Recompute the path lengths and moment arms.
    TimeSeriesTable pathLengthsFitted;
    TimeSeriesTable momentArmsFitted;
    computePathLengthsAndMomentArms(modelFitted, m_values, forcePaths,
            momentArmMap, settings, pathLengthsFitted, momentArmsFitted);

    TimeSeriesTable pathLengthsSampledFitted;
    TimeSeriesTable momentArmsSampledFitted;
    computePathLengthsAndMomentArms(modelFitted, valuesSampled, forcePaths,
            momentArmMap, settings, pathLengthsSampledFitted,
            momentArmsSampledFitted);

    // Compute the RMS error between the original and fitted path lengths and
    // moment arms.
    computeFittingErrors(modelFitted, pathLengthsSampled, momentArmsSampled,
            pathLengthsSampledFitted, momentArmsSampledFitted, settings);

    log_info("");
    log_info("Step 9/9: Print the results to the output directory.");
    log_info("----------------------------------------------------");

    // Print the FunctionBasedPaths to file.
    std::string functionBasedPathsFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_FunctionBasedPathSet.xml",
                            outputDir, m_model.getName()));
    log_info("Printing the FunctionBasedPaths to '{}'...",
            functionBasedPathsFileName);
    functionBasedPaths.print(functionBasedPathsFileName);

    // Print the coordinate values to file.
    std::string coordinatesFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_coordinate_values.sto",
                            outputDir, m_model.getName()));
    std::string sampledCoordinatesFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_coordinate_values_sampled.sto",
                            outputDir, m_model.getName()));
    log_info("");
    log_info(fmt::format("Printing original coordinate values to '{}'...",
            coordinatesFileName));
    STOFileAdapter::write(m_values, coordinatesFileName);
    log_info(fmt::format("Printing sampled coordinate values to '{}'...",
            sampledCoordinatesFileName));
    STOFileAdapter::write(valuesSampled, sampledCoordinatesFileName);

    // Print the path lengths and moment arms from the original coordinate
    // data to file.
    std::string pathLengthsFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_path_lengths.sto",
                            outputDir, m_model.getName()));
    std::string momentArmsFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_moment_arms.sto",
                            outputDir, m_model.getName()));
    log_info("");
    log_info("Printing the path lengths to '{}'...",
            pathLengthsFileName);
    STOFileAdapter::write(pathLengths, pathLengthsFileName);
    log_info("Printing the moment arms to '{}'...", momentArmsFileName);
    STOFileAdapter::write(momentArms, momentArmsFileName);

    // Print the path lengths and moment arms from the sampled coordinate
    // data to file.
    std::string pathLengthsSampledFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_path_lengths_sampled.sto",
                            outputDir, m_model.getName()));
    std::string momentArmsSampledFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_moment_arms_sampled.sto",
                            outputDir, m_model.getName()));

    log_info("");
    log_info("Printing the sampled path lengths to '{}'...",
            pathLengthsSampledFileName);
    STOFileAdapter::write(pathLengthsSampled, pathLengthsSampledFileName);
    log_info("Printing the sampled moment arms to '{}'...",
            momentArmsSampledFileName);
    STOFileAdapter::write(momentArmsSampled, momentArmsSampledFileName);

    // Print the fitted path lengths and moment arms using the original
    // coordinate data to file.
    std::string pathLengthsFittedFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_path_lengths_fitted.sto",
                            outputDir, m_model.getName()));
    std::string momentArmsFittedFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_moment_arms_fitted.sto",
                            outputDir, m_model.getName()));

    log_info("");
    log_info("Printing the fitted path lengths from the original coordinate "
             "values to '{}'...", pathLengthsFittedFileName);
    STOFileAdapter::write(pathLengthsFitted, pathLengthsFittedFileName);
    log_info("Printing the fitted moment arms from the original coordinate "
             "to '{}'...", momentArmsFittedFileName);
    STOFileAdapter::write(momentArmsFitted, momentArmsFittedFileName);

    // Print the fitted path lengths and moment arms using the sampled
    // coordinate data to file.
    std::string pathLengthsSampledFittedFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_path_lengths_sampled_fitted.sto",
                            outputDir, m_model.getName()));
    std::string momentArmsSampledFittedFileName =
            SimTK::Pathname::getAbsolutePathname(
                    fmt::format("{}/{}_moment_arms_sampled_fitted.sto",
                            outputDir, m_model.getName()));

    log_info("");
    log_info("Printing the fitted path lengths from the sampled coordinate "
             "values to '{}'...", pathLengthsSampledFittedFileName);
    STOFileAdapter::write(pathLengthsSampledFitted,
            pathLengthsSampledFittedFileName);
    log_info("Printing the fitted moment arms from the sampled coordinate "
             "values to '{}'...", momentArmsSampledFittedFileName);
    STOFileAdapter::write(momentArmsSampledFitted,
            momentArmsSampledFittedFileName);
}



//=============================================================================
// SETTINGS
//=============================================================================

void PolynomialPathFitter::setModel(ModelProcessor model) {
    set_model(std::move(model));
}

void PolynomialPathFitter::setCoordinateValues(TableProcessor values) {
    set_coordinate_values(std::move(values));
}

void PolynomialPathFitter::setOutputDirectory(std::string directory) {
    set_output_directory(std::move(directory));
}

std::string PolynomialPathFitter::getOutputDirectory() const {
    return get_output_directory();
}

void PolynomialPathFitter::setUseStepwiseRegression(bool tf) {
    set_use_stepwise_regression(tf);
}

bool PolynomialPathFitter::getUseStepwiseRegression() const {
    return get_use_stepwise_regression();
}

void PolynomialPathFitter::setMinimumPolynomialOrder(int order) {
    set_minimum_polynomial_order(order);
}

int PolynomialPathFitter::getMinimumPolynomialOrder() const {
    return get_minimum_polynomial_order();
}

void PolynomialPathFitter::setMaximumPolynomialOrder(int order) {
    set_maximum_polynomial_order(order);
}

int PolynomialPathFitter::getMaximumPolynomialOrder() const {
    return get_maximum_polynomial_order();
}

void PolynomialPathFitter::setGlobalCoordinateSamplingBounds(SimTK::Vec2 bounds)
{
    set_global_coordinate_sampling_bounds(std::move(bounds));
}

SimTK::Vec2 PolynomialPathFitter::getGlobalCoordinateSamplingBounds() const {
    return get_global_coordinate_sampling_bounds();
}

void PolynomialPathFitter::appendCoordinateSamplingBounds(
        const std::string& coordinatePath, const SimTK::Vec2& bounds) {
    append_coordinate_sampling_bounds({coordinatePath, bounds});
}

void PolynomialPathFitter::setMomentArmTolerance(double tolerance) {
    set_moment_arm_tolerance(tolerance);
}

double PolynomialPathFitter::getMomentArmTolerance() const {
    return get_moment_arm_tolerance();
}

void PolynomialPathFitter::setPathLengthTolerance(double tolerance) {
    set_path_length_tolerance(tolerance);
}

double PolynomialPathFitter::getPathLengthTolerance() const {
    return get_path_length_tolerance();
}

void PolynomialPathFitter::setNumSamplesPerFrame(int numSamples) {
    set_num_samples_per_frame(numSamples);
}

int PolynomialPathFitter::getNumSamplesPerFrame() const {
    return get_num_samples_per_frame();
}

void PolynomialPathFitter::setNumParallelThreads(int numThreads) {
    set_num_parallel_threads(numThreads);
}

int PolynomialPathFitter::getNumParallelThreads() const {
    return get_num_parallel_threads();
}

void PolynomialPathFitter::setLatinHypercubeAlgorithm(
        std::string algorithm) {
    set_latin_hypercube_algorithm(std::move(algorithm));
}

std::string PolynomialPathFitter::getLatinHypercubeAlgorithm() const {
    return get_latin_hypercube_algorithm();
}

//=============================================================================
// HELPER FUNCTIONS
//=============================================================================

void PolynomialPathFitter::evaluateFunctionBasedPaths(Model model,
        TableProcessor trajectory,
        const std::string& functionBasedPathsFileName,
        double pathLengthTolerance, double momentArmTolerance) {

    Settings settings;
    settings.pathLengthTolerance = pathLengthTolerance;
    settings.momentArmTolerance = momentArmTolerance;
    settings.numParallelThreads =
            static_cast<int>(std::thread::hardware_concurrency());
    OPENSIM_THROW_IF(settings.numParallelThreads == 0, Exception,
            "Tried to evaluate the FunctionBasedPaths, but no hardware threads "
            "were found.");

    // Initialize the original model.
    model.initSystem();

    // Get the path-based forces in the model.
    std::vector<std::string> forcePaths;
    const auto& forces = model.getComponentList<Force>();
    for (const auto& force : forces) {
        if (force.hasProperty("path")) {
            forcePaths.push_back(force.getAbsolutePathString());
        }
    }

    // Create another model and add the FunctionBasedPaths to it.
    ModelProcessor modelProcessor = ModelProcessor(model) |
            ModOpReplacePathsWithFunctionBasedPaths(functionBasedPathsFileName);
    Model modelFitted = modelProcessor.process();

    // Create a moment arm map based on the fitted model paths.
    MomentArmMap momentArmMap;
    Set<FunctionBasedPath> functionBasedPaths(functionBasedPathsFileName);
    for (int i = 0; i < functionBasedPaths.getSize(); ++i) {
        const auto& path = functionBasedPaths.get(i);
        momentArmMap[path.getName()] = path.getCoordinatePaths();
    }

    // Get the coordinate values table from the trajectory. This may contain
    // extra columns, but that's okay.
    std::string currentDirectory = IO::getCwd();
    TimeSeriesTable coordinateValues = loadCoordinateValuesAndValidateModel(
            currentDirectory, std::move(trajectory), model);
    OPENSIM_THROW_IF(coordinateValues.getNumRows() == 0, Exception,
            "Expected the coordinate values table to contain at least one row, "
            "but it is empty.");

    // Compute path lengths and moment arms for the original and fitted models.
    log_info("");
    log_info("Computing path lengths and moment arms for the original model..");
    TimeSeriesTable pathLengths;
    TimeSeriesTable momentArms;
    computePathLengthsAndMomentArms(model, coordinateValues, forcePaths,
            momentArmMap, settings, pathLengths, momentArms);

    log_info("");
    log_info("Computing path lengths and moment arms for the fitted model..");
    TimeSeriesTable pathLengthsFitted;
    TimeSeriesTable momentArmsFitted;
    computePathLengthsAndMomentArms(modelFitted, coordinateValues, forcePaths,
            momentArmMap, settings, pathLengthsFitted, momentArmsFitted);

    // Compute the RMS errors.
    computeFittingErrors(modelFitted, pathLengths, momentArms,
            pathLengthsFitted, momentArmsFitted, settings);
}

void PolynomialPathFitter::constructProperties() {
    constructProperty_model(ModelProcessor());
    constructProperty_coordinate_values(TableProcessor());
    constructProperty_output_directory("");
    constructProperty_use_stepwise_regression(false);
    constructProperty_moment_arm_tolerance(1e-4);
    constructProperty_path_length_tolerance(1e-4);
    constructProperty_minimum_polynomial_order(2);
    constructProperty_maximum_polynomial_order(6);
    constructProperty_num_parallel_threads(
            (int)std::thread::hardware_concurrency());
    constructProperty_global_coordinate_sampling_bounds({-10.0, 10.0});
    constructProperty_coordinate_sampling_bounds();
    constructProperty_num_samples_per_frame(25);
    constructProperty_latin_hypercube_algorithm("random");
    constructProperty_include_moment_arm_functions(false);
    constructProperty_include_lengthening_speed_function(false);
}

void PolynomialPathFitter::validateProperties() {
    // Parallelization.
    OPENSIM_THROW_IF_FRMOBJ(std::thread::hardware_concurrency() == 0, Exception,
            "Expected at least one hardware thread for parallelization, but "
            "none were found.")
    OPENSIM_THROW_IF_FRMOBJ(get_num_parallel_threads() < 1 ||
            get_num_parallel_threads() >
                (int)std::thread::hardware_concurrency(), Exception,
            "Expected 'num_parallel_threads' to be between 1 and {}, but "
            "received {}.", std::thread::hardware_concurrency(),
            get_num_parallel_threads())
    OPENSIM_THROW_IF_FRMOBJ(
            static_cast<int>(m_values.getNumRows()) < get_num_parallel_threads(),
            Exception, "Expected the number of time points in the coordinate "
            "values table to be greater than 'num_parallel_threads', but "
            "received {} and {}, respectively.",
            m_values.getNumRows(), get_num_parallel_threads())
    log_info("Number of parallel threads = {}", get_num_parallel_threads());

    // Number of samples per frame.
    OPENSIM_THROW_IF_FRMOBJ(get_num_samples_per_frame() < 1, Exception,
            "Expected 'num_samples_per_frame' to be a non-zero integer value, "
            "but received {}.", get_num_samples_per_frame());
    log_info("Number of samples per frame = {}", get_num_samples_per_frame());

    // Latin hypercube algorithm.
    checkPropertyValueIsInSet(getProperty_latin_hypercube_algorithm(),
            {"random", "ESEA"});
    log_info("Latin hypercube algorithm = '{}'",
            get_latin_hypercube_algorithm());

    // Polynomial order.
    checkPropertyValueIsPositive(getProperty_minimum_polynomial_order());
    checkPropertyValueIsPositive(getProperty_maximum_polynomial_order());
    OPENSIM_THROW_IF_FRMOBJ(get_maximum_polynomial_order() > 9, Exception,
            "Expected 'maximum_polynomial_order' to be at most 9, but "
            "received {}.", get_maximum_polynomial_order())
    OPENSIM_THROW_IF_FRMOBJ(get_minimum_polynomial_order() >
                            get_maximum_polynomial_order(), Exception,
            "Expected 'minimum_polynomial_order' to be less than or equal to "
            "'maximum_polynomial_order', but received {} and {}, "
            "respectively.", get_minimum_polynomial_order(),
            get_maximum_polynomial_order())
    log_info("Minimum polynomial order = {}", get_minimum_polynomial_order());
    log_info("Maximum polynomial order = {}", get_maximum_polynomial_order());

    // Fitting tolerances.
    checkPropertyValueIsInRangeOrSet(getProperty_path_length_tolerance(),
                                     0.0, 1.0, {});
    checkPropertyValueIsInRangeOrSet(getProperty_moment_arm_tolerance(),
                                     0.0, 1.0, {});
    log_info("Path length fitting tolerance = {:1.1e} meters",
             get_path_length_tolerance(), 1);
    log_info("Moment arm fitting tolerance = {:1.1e} meters",
            get_moment_arm_tolerance(), 1);

    // Stepwise regression
    log_info("Use stepwise regression = {}",
            get_use_stepwise_regression() ? "true" : "false");
}

std::string PolynomialPathFitter::getDocumentDirectory() const {
    std::string setupDir;
    {
        bool dontApplySearchPath;
        std::string fileName, extension;
        SimTK::Pathname::deconstructPathname(getDocumentFileName(),
                dontApplySearchPath, setupDir, fileName, extension);
    }
    return setupDir;
}