diff --git a/CHANGELOG.md b/CHANGELOG.md index c411641ae1..9edd754f29 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,14 @@ v4.6.1 - Added `CoordinateLinearStopForce`, a force element for enforcing coordinate limits using a linear spring force and Hunt and Crossley-like damping. (#4329) - Updated `ExponentialCoordinateLimitForce` to use `Socket` to define the connection to a particular `Coordinate`. (#4329) +- Added `SimulationUtilities::findJointsBetweenPhysicalFrames`, a utility function for finding the set of +joints that lie between two frames based on a model's topology. (#4301) +- Added `AbstractGeometryPath::findIndependentCoordinatePaths()`, a utility method for finding the list of +coordinate paths associated with a path element. Derived classes provide concrete implementations based on +the model's topology (e.g., `GeometryPath`) or via a user-defined list of coordinates +(e.g., `FunctionBasedPath`). (#4301) +- `PolynomialPathFitter` now detects which coordinates should belong to a path based on the model topology by finding the joints that lie between the path's origin and insertion, and computes moment arms only for these coordinates. +The property `moment_arm_threshold` has been removed, and the methods `get/setMomentArmThreshold()` have been deprecated. (#4352) v4.6 diff --git a/OpenSim/Actuators/PolynomialPathFitter.cpp b/OpenSim/Actuators/PolynomialPathFitter.cpp index 3a49dfebb4..3e923ac278 100644 --- a/OpenSim/Actuators/PolynomialPathFitter.cpp +++ b/OpenSim/Actuators/PolynomialPathFitter.cpp @@ -32,8 +32,8 @@ #include #include #include -#include +#include #include using namespace OpenSim; @@ -41,8 +41,8 @@ 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 names of coordinates on which -// the paths depend. +// and the values are vectors containing the (model) paths to coordinates on +// which the (wrapping) paths depend. using MomentArmMap = std::unordered_map>; // Type alias for the coordinate bounds and coordinate range maps. The keys are @@ -54,7 +54,6 @@ using CoordinateMap = std::unordered_map; // the user via the PolynomialPathFitter properties. struct Settings { bool useStepwiseRegression = false; - double momentArmThreshold = 1e-3; double momentArmTolerance = 1e-4; double pathLengthTolerance = 1e-4; int minimumPolynomialOrder = 2; @@ -324,186 +323,147 @@ void computePathLengthsAndMomentArms( const Model& model, const TimeSeriesTable& coordinateValues, const std::vector& forcePaths, + const MomentArmMap& momentArmMap, const Settings& settings, TimeSeriesTable& pathLengths, TimeSeriesTable& momentArms) { - // Determine the number of threads to use for the path length and moment - // arm computations. - const int numTimePoints = static_cast(coordinateValues.getNumRows()); - OPENSIM_THROW_IF(numTimePoints == 0, Exception, + // Precalculate some useful values for the path length and moment arm + // computations. + const int numTimes = static_cast(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 numThreads = std::min(numTimePoints, settings.numParallelThreads); - - // Create a StatesTrajectory from the coordinate values. - auto statesTrajectory = StatesTrajectory::createFromStatesTable( - model, coordinateValues, true, false, false); + const int numPaths = static_cast(forcePaths.size()); + const int numThreads = std::min(numPaths, settings.numParallelThreads); + int numMomentArms = 0; + for (const auto& [forcePath, coordinatePaths] : momentArmMap) { + numMomentArms += coordinatePaths.size(); + } - // Determine the maximum number of path and moment arm evaluations. - const auto& paths = model.getComponentList(); - int numPaths = (int)std::distance(paths.begin(), paths.end()); - int numCoordinates = (int)coordinateValues.getNumColumns(); - int numColumns = numPaths + (numPaths * numCoordinates); - - // Define helper function for path length and moment arm computations. - auto calcPathLengthsAndMomentArmsSubset = - [numThreads, numColumns, numPaths, forcePaths]( - Model model, int thread, - StatesTrajectory::IteratorRange subsetStates) -> SimTK::Matrix { + // Define the path calculation worker. The atomic integer 'nextPathIndex' + // controls the path from a thread should perform length and moment arm + // calculations. + std::atomic nextPathIndex(0); + std::vector pathResults(numPaths); + auto pathCalculationWorker = [&](Model model, StatesTrajectory statesTraj) { model.initSystem(); - int numTimePoints = (int)std::distance(subsetStates.begin(), - subsetStates.end()); - log_info("Thread {:2d}/{:2d}: computing values for times " - "{:1.2f}-{:1.2f} seconds...", thread+1, numThreads, - subsetStates.begin()->getTime(), - (subsetStates.end()-1)->getTime()); - - SimTK::Matrix results(numTimePoints, numColumns); - int row = 0; - for (const auto& state : subsetStates) { - - // Attempt to compute path lengths and moment arms for the current - // state. If an error occurs, set the path lengths and moment arms - // to NaN. This sample will be filtered out later on before fitting - // the polynomial coefficients. - try { - model.realizePosition(state); - int ip = 0; - int ima = 0; - for (const auto& forcePath : forcePaths) { - const auto& force = model.getComponent(forcePath); - const AbstractGeometryPath& path = - force.getPropertyByName("path") + 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(forcePath); + const AbstractGeometryPath& path = + force.getPropertyByName("path") .getValue(); + std::vector> coordinates; + for (const auto& coordinatePath : momentArmMap.at(forcePath)) { + coordinates.emplace_back( + &model.getComponent(coordinatePath)); + } + const int numCoordsThisPath = static_cast(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(row, ip++) = path.getLength(state); + results(irow, 0) = path.getLength(state); // Compute moment arms. - for (const auto& coordinate : - model.getComponentList()) { - results(row, numPaths + ima++) = - path.computeMomentArm(state, coordinate); + 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(); } - } catch (const std::exception& e) { - log_warn("Skipping sample at time {:1.2f} s due to error in " - "path computation: {}", state.getTime(), e.what()); - // Inject a NaN value in the row associated with this sample - // so that it is filtered out later on before fitting the - // polynomial coefficients. - results(row, 0) = SimTK::NaN; - results(row, numPaths) = SimTK::NaN; } - - // Update the row index. - row++; } - - return results; }; - // Divide the path length and moment arm computations across multiple - // threads. - int offset = 0; - int stride = static_cast( - std::floor(coordinateValues.getNumRows() / numThreads)); - std::vector> futures; + // Construct a StatesTrajectory from the coordinates table. + auto statesTraj = StatesTrajectory::createFromStatesTable( + model, coordinateValues, true, false, false); + OPENSIM_ASSERT_ALWAYS(numTimes == static_cast(statesTraj.getSize())); + + // Launch the worker threads. + std::vector> futures; + futures.reserve(numThreads); for (int thread = 0; thread < numThreads; ++thread) { - auto begin_iter = statesTrajectory.begin() + offset; - auto end_iter = (thread == numThreads-1) ? - statesTrajectory.end() : - statesTrajectory.begin() + offset + stride; futures.push_back(std::async(std::launch::async, - calcPathLengthsAndMomentArmsSubset, - model, thread, makeIteratorRange(begin_iter, end_iter))); - offset += stride; + pathCalculationWorker, model, statesTraj)); } - // Wait for threads to finish and collect results - std::vector outputs; - outputs.reserve(numThreads); - for (int i = 0; i < numThreads; ++i) { - outputs.push_back(futures[i].get()); + // Wait for all threads to finish. + for (auto& future : futures) { + future.get(); } - // Assemble results into one TimeSeriesTable - std::vector times = coordinateValues.getIndependentColumn(); - int itime = 0; - for (int i = 0; i < numThreads; ++i) { - for (int j = 0; j < outputs[i].nrow(); ++j) { - pathLengths.appendRow(times[itime], - outputs[i].block(j, 0, 1, numPaths).getAsRowVector()); - momentArms.appendRow(times[itime], outputs[i].block(j, numPaths, 1, - numPaths * numCoordinates).getAsRowVector()); - itime++; - } + // 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; } - int ip = 0; - int ima = 0; - std::vector pathLengthLabels(numPaths); - std::vector momentArmLabels(numPaths * numCoordinates); + // Assemble the results into TimeSeriesTables. + std::vector pathLengthLabels; + pathLengthLabels.reserve(numPaths); + std::vector momentArmLabels; + momentArmLabels.reserve(numMomentArms); for (const auto& forcePath : forcePaths) { - const auto& force = model.getComponent(forcePath); - pathLengthLabels[ip++] = - fmt::format("{}_length", force.getAbsolutePathString()); - for (const auto& coordinate : - model.getComponentList()) { - momentArmLabels[ima++] = fmt::format("{}_moment_arm_{}", - force.getAbsolutePathString(), coordinate.getName()); + 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())); } } - pathLengths.setColumnLabels(pathLengthLabels); - momentArms.setColumnLabels(momentArmLabels); + const std::vector& times = coordinateValues.getIndependentColumn(); + pathLengths = TimeSeriesTable(times, lengthsMatrix, pathLengthLabels); + momentArms = TimeSeriesTable(times, momentArmsMatrix, momentArmLabels); } -// Helper function to filter out bad coordinate value samples and determine -// which coordinates each path is dependent on. Bad samples are defined as -// coordinate values that produce path length and/or moment are values that -// deviate by a set number of standard deviations away from the nominal -// trajectories. The `momentArmMap` argument is a map containing the -// coordinates each path is dependent on. Columns in the `momentArms` table -// are removed if they do not correspond to entries in the `momentArmMap`. +// 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 Model& model, const Settings& settings, TimeSeriesTable& coordinateValues, TimeSeriesTable& pathLengths, - TimeSeriesTable& momentArms, - MomentArmMap& momentArmMap) { - - // Remove moment arm columns for coupled coordinates. - for (const auto& couplerConstraint : - model.getComponentList()) { - auto momentArmLabel = fmt::format("_moment_arm_{}", - couplerConstraint.getDependentCoordinateName()); - for (const auto& label : momentArms.getColumnLabels()) { - if (label.find(momentArmLabel) != std::string::npos) { - momentArms.removeColumn(label); - } - } - } - - // Remove moment arm columns that contain values below the specified - // moment arm tolerance. - for (const auto& label : momentArms.getColumnLabels()) { - if (label.find("_moment_arm_") != std::string::npos) { - const auto& col = momentArms.getDependentColumn(label); - bool removeColumn = col.normInf() < settings.momentArmThreshold; - std::string path = label.substr(0, label.find("_moment_arm_")); - std::string coordinate = label.substr( - label.find("_moment_arm_") + 12); - - if (removeColumn) { - momentArms.removeColumn(label); - } else { - momentArmMap[path].push_back(coordinate); - } - } - } + TimeSeriesTable& momentArms) { // Remove any path length or moment arm rows that contain NaN values. std::vector times = coordinateValues.getIndependentColumn(); @@ -820,7 +780,6 @@ void fitCoefficientsStepwiseRegression( // that each `MultivariatePolynomialFunction` contains to approximate the // original path. Set fitPolynomialCoefficients( - const Model& model, const TimeSeriesTable& coordinateValues, const std::vector& forcePaths, const TimeSeriesTable& pathLengths, @@ -828,9 +787,6 @@ Set fitPolynomialCoefficients( const MomentArmMap& momentArmMap, const Settings& settings) { - // Coordinate references. - // ---------------------- - const CoordinateSet& coordinateSet = model.getCoordinateSet(); const int numTimes = (int)coordinateValues.getNumRows(); // Pre-compute variables. @@ -868,17 +824,10 @@ Set fitPolynomialCoefficients( "'{}'...", thread+1, settings.numParallelThreads, forcePath); - // The current force path and the number of coordinates it depends - // on. - std::vector coordinatesNamesThisForce = + // The coordinate paths that the current force depends on. + const std::vector& coordinatePathsThisForce = momentArmMap.at(forcePath); - int numCoordinatesThisForce = (int)coordinatesNamesThisForce.size(); - std::vector coordinatePathsThisForce; - coordinatePathsThisForce.reserve(numCoordinatesThisForce); - for (const auto& coordinateName : coordinatesNamesThisForce) { - coordinatePathsThisForce.push_back( - coordinateSet.get(coordinateName).getAbsolutePathString()); - } + int numCoordinatesThisForce = (int)coordinatePathsThisForce.size(); // Initialize the 'b' vector. This is the same for all polynomial // orders. @@ -895,8 +844,8 @@ Set fitPolynomialCoefficients( SimTK::Matrix coordinatesThisForce( numTimes, numCoordinatesThisForce, 0.0); for (int ic = 0; ic < numCoordinatesThisForce; ++ic) { - const std::string& coordinateName = - coordinatesNamesThisForce[ic]; + const std::string coordinateName = ComponentPath( + coordinatePathsThisForce[ic]).getComponentName(); b((ic+1)*numTimes, numTimes) = momentArms.getDependentColumn( fmt::format("{}_moment_arm_{}", forcePath, coordinateName)); @@ -987,28 +936,6 @@ Set fitPolynomialCoefficients( return functionBasedPaths; } -// Remove columns from the `momentArms` table that do not correspond to -// entries in the `momentArmMap`. -void removeMomentArmColumns(TimeSeriesTable& momentArms, - const MomentArmMap& momentArmMap) { - - // Remove entries from the table that are not in the moment arm map. - for (const auto& label : momentArms.getColumnLabels()) { - std::string path = label.substr(0, label.find("_moment_arm_")); - std::string coordinate = label.substr( - label.find("_moment_arm_") + 12); - if (momentArmMap.find(path) != momentArmMap.end()) { - if (std::find(momentArmMap.at(path).begin(), - momentArmMap.at(path).end(), coordinate) == - momentArmMap.at(path).end()) { - momentArms.removeColumn(label); - } - } else { - momentArms.removeColumn(label); - } - } -} - // 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. @@ -1192,7 +1119,7 @@ void PolynomialPathFitter::run() { // Load the model. m_model = get_model().process(getDocumentDirectory()); - m_model.initSystem(); + SimTK::State state = m_model.initSystem(); // Load the coordinate values table. m_values = loadCoordinateValuesAndValidateModel( @@ -1201,14 +1128,33 @@ void PolynomialPathFitter::run() { "Expected the coordinate values table to contain at least one row, " "but it is empty."); - // Get the path-based forces in the model. + // Get the path-based forces in the model and construct the moment arm + // map based on the coordinates each path depends on. std::vector forcePaths; + MomentArmMap momentArmMap; const auto& forces = m_model.getComponentList(); for (const auto& force : forces) { if (force.hasProperty("path")) { - forcePaths.push_back(force.getAbsolutePathString()); + const std::string forcePath = force.getAbsolutePathString(); + const AbstractGeometryPath& path = + force.getPropertyByName("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."); @@ -1269,7 +1215,6 @@ void PolynomialPathFitter::run() { Settings settings; settings.useStepwiseRegression = get_use_stepwise_regression(); - settings.momentArmThreshold = get_moment_arm_threshold(); settings.momentArmTolerance = get_moment_arm_tolerance(); settings.pathLengthTolerance = get_path_length_tolerance(); settings.minimumPolynomialOrder = get_minimum_polynomial_order(); @@ -1314,7 +1259,7 @@ void PolynomialPathFitter::run() { TimeSeriesTable pathLengths; TimeSeriesTable momentArms; computePathLengthsAndMomentArms(m_model, m_values, forcePaths, - settings, pathLengths, momentArms); + momentArmMap, settings, pathLengths, momentArms); log_info(""); log_info("Computing path lengths and moment arms for the sampled " @@ -1322,20 +1267,19 @@ void PolynomialPathFitter::run() { TimeSeriesTable pathLengthsSampled; TimeSeriesTable momentArmsSampled; computePathLengthsAndMomentArms(m_model, valuesSampled, forcePaths, - settings, pathLengthsSampled, momentArmsSampled); + momentArmMap, settings, pathLengthsSampled, momentArmsSampled); log_info(""); log_info("Step 6/9: Filter the sampled path data."); log_info("---------------------------------------"); - MomentArmMap momentArmMap; - filterSampledData(m_model, settings, valuesSampled, pathLengthsSampled, - momentArmsSampled, momentArmMap); + filterSampledData(settings, valuesSampled, pathLengthsSampled, + momentArmsSampled); log_info(""); log_info("Step 7/9: Fit the polynomial coefficients."); log_info("------------------------------------------"); Set functionBasedPaths = fitPolynomialCoefficients( - m_model, valuesSampled, forcePaths, pathLengthsSampled, + valuesSampled, forcePaths, pathLengthsSampled, momentArmsSampled, momentArmMap, settings); Array pathNames; functionBasedPaths.getNames(pathNames); @@ -1394,19 +1338,14 @@ void PolynomialPathFitter::run() { TimeSeriesTable pathLengthsFitted; TimeSeriesTable momentArmsFitted; computePathLengthsAndMomentArms(modelFitted, m_values, forcePaths, - settings, pathLengthsFitted, momentArmsFitted); + momentArmMap, settings, pathLengthsFitted, momentArmsFitted); TimeSeriesTable pathLengthsSampledFitted; TimeSeriesTable momentArmsSampledFitted; computePathLengthsAndMomentArms(modelFitted, valuesSampled, forcePaths, - settings, pathLengthsSampledFitted, + momentArmMap, settings, pathLengthsSampledFitted, momentArmsSampledFitted); - // Remove moment arm columns that are not in the map. - removeMomentArmColumns(momentArms, momentArmMap); - removeMomentArmColumns(momentArmsFitted, momentArmMap); - removeMomentArmColumns(momentArmsSampledFitted, momentArmMap); - // Compute the RMS error between the original and fitted path lengths and // moment arms. computeFittingErrors(modelFitted, pathLengthsSampled, momentArmsSampled, @@ -1549,14 +1488,6 @@ bool PolynomialPathFitter::getUseStepwiseRegression() const { return get_use_stepwise_regression(); } -void PolynomialPathFitter::setMomentArmThreshold(double threshold) { - set_moment_arm_threshold(threshold); -} - -double PolynomialPathFitter::getMomentArmThreshold() const { - return get_moment_arm_threshold(); -} - void PolynomialPathFitter::setMinimumPolynomialOrder(int order) { set_minimum_polynomial_order(order); } @@ -1668,14 +1599,7 @@ void PolynomialPathFitter::evaluateFunctionBasedPaths(Model model, Set functionBasedPaths(functionBasedPathsFileName); for (int i = 0; i < functionBasedPaths.getSize(); ++i) { const auto& path = functionBasedPaths.get(i); - std::vector coordinateNames; - for (const auto& coordinatePath : path.getCoordinatePaths()) { - const auto& coordinate = - model.getComponent(coordinatePath); - coordinateNames.push_back(coordinate.getName()); - } - - momentArmMap[path.getName()] = coordinateNames; + momentArmMap[path.getName()] = path.getCoordinatePaths(); } // Get the coordinate values table from the trajectory. This may contain @@ -1693,18 +1617,14 @@ void PolynomialPathFitter::evaluateFunctionBasedPaths(Model model, TimeSeriesTable pathLengths; TimeSeriesTable momentArms; computePathLengthsAndMomentArms(model, coordinateValues, forcePaths, - settings, pathLengths, momentArms); + 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, - settings, pathLengthsFitted, momentArmsFitted); - - // Remove moment arm columns that are not in the map. - removeMomentArmColumns(momentArms, momentArmMap); - removeMomentArmColumns(momentArmsFitted, momentArmMap); + momentArmMap, settings, pathLengthsFitted, momentArmsFitted); // Compute the RMS errors. computeFittingErrors(modelFitted, pathLengths, momentArms, @@ -1716,7 +1636,6 @@ void PolynomialPathFitter::constructProperties() { constructProperty_coordinate_values(TableProcessor()); constructProperty_output_directory(""); constructProperty_use_stepwise_regression(false); - constructProperty_moment_arm_threshold(1e-3); constructProperty_moment_arm_tolerance(1e-4); constructProperty_path_length_tolerance(1e-4); constructProperty_minimum_polynomial_order(2); @@ -1762,14 +1681,6 @@ void PolynomialPathFitter::validateProperties() { log_info("Latin hypercube algorithm = '{}'", get_latin_hypercube_algorithm()); - // Moment arm threshold. - OPENSIM_THROW_IF_FRMOBJ(get_moment_arm_threshold() < 0 || - get_moment_arm_threshold() > 1, Exception, - "Expected 'moment_arm_threshold' to be in the range [0, 1], but " - "received {:2g}.", get_moment_arm_threshold()) - log_info("Moment arm threshold = {:1.1e} meters", - get_moment_arm_threshold(), 1); - // Polynomial order. checkPropertyValueIsPositive(getProperty_minimum_polynomial_order()); checkPropertyValueIsPositive(getProperty_maximum_polynomial_order()); diff --git a/OpenSim/Actuators/PolynomialPathFitter.h b/OpenSim/Actuators/PolynomialPathFitter.h index 45aa7dc80e..2654f8d626 100644 --- a/OpenSim/Actuators/PolynomialPathFitter.h +++ b/OpenSim/Actuators/PolynomialPathFitter.h @@ -75,29 +75,24 @@ class OSIMACTUATORS_API PolynomialPathFitterBounds : public Object { * * # Settings * Various settings can be adjusted to control the path fitting process. The - * `setMomentArmsThreshold` method determines whether or not a path depends on a - * model coordinate. In other words, the absolute value the moment arm of a with - * respect to a particular coordinate must be greater than this value to be - * included during path fitting. The `setMinimumPolynomialOrder` and - * `setMaximumPolynomialOrder` methods specify the minimum and maximum order of - * the polynomial used to fit each path. The `setGlobalCoordinateSamplingBounds` - * property specifies the global bounds (in degrees) that determine the minimum - * and maximum coordinate values sampled at each time point. The method - * `appendCoordinateSamplingBounds` can be used to override the global bounds - * for a specific coordinate. The `setMomentArmTolerance` and - * `setPathLengthTolerance` methods specify the tolerance on the - * root-mean-square (RMS) error (in meters) between the moment arms and path - * lengths computed from the original model paths and the fitted polynomial - * paths. The `setNumSamplesPerFrame` method specifies the number of samples - * taken per time frame in the coordinate values table used to fit each path. - * The `setNumParallelThreads` method specifies the number of threads used to - * parallelize the path fitting process. The `setLatinHypercubeAlgorithm` method - * specifies the Latin hypercube sampling algorithm used to sample coordinate - * values for path fitting. + * `setMinimumPolynomialOrder` and `setMaximumPolynomialOrder` methods specify + * the minimum and maximum order of the polynomial used to fit each path. The + * `setGlobalCoordinateSamplingBounds` property specifies the global bounds (in + * degrees) that determine the minimum and maximum coordinate values sampled at + * each time point. The method `appendCoordinateSamplingBounds` can be used to + * override the global bounds for a specific coordinate. The + * `setMomentArmTolerance` and `setPathLengthTolerance` methods specify the + * tolerance on the root-mean-square (RMS) error (in meters) between the moment + * arms and path lengths computed from the original model paths and the fitted + * polynomial paths. The `setNumSamplesPerFrame` method specifies the number of + * samples taken per time frame in the coordinate values table used to fit each + * path. The `setNumParallelThreads` method specifies the number of threads used + * to parallelize the path fitting process. The `setLatinHypercubeAlgorithm` + * method specifies the Latin hypercube sampling algorithm used to sample + * coordinate values for path fitting. * * The default settings are as follows: * - * - Moment arm threshold: 1e-3 meters * - Minimum polynomial order: 2 * - Maximum polynomial order: 6 * - Global coordinate sampling bounds: [-10, 10] degrees @@ -297,18 +292,6 @@ class OSIMACTUATORS_API PolynomialPathFitter : public Object { void setUseStepwiseRegression(bool tf); bool getUseStepwiseRegression() const; - /** - * The moment arm threshold value that determines whether or not a path - * depends on a model coordinate. In other words, the moment arm of a path - * with respect to a particular coordinate must be greater than this value - * to be included during path fitting. - * - * @note The default moment arm threshold is set to 1e-3 meters. - */ - void setMomentArmThreshold(double threshold); - /// @copydoc setMomentArmThreshold() - double getMomentArmThreshold() const; - /** * The minimum order of the polynomial used to fit each path. The order of * a polynomial is the highest power of the independent variable(s) in the @@ -492,6 +475,15 @@ class OSIMACTUATORS_API PolynomialPathFitter : public Object { return get_include_lengthening_speed_function(); } + /// (Deprecated) Moment arms associated with a path are now + /// automatically detected based on the model's topology. + [[deprecated("Path moment arms are now detected based on model topology.")]] + void setMomentArmThreshold(double) {} + /// (Deprecated) Moment arms associated with a path are now + /// automatically detected based on the model's topology. + [[deprecated("Path moment arms are now detected based on model topology.")]] + double getMomentArmThreshold() const { return -1; } + // HELPER FUNCTIONS /** * Print out a summary of the path fitting results, including information @@ -530,11 +522,6 @@ class OSIMACTUATORS_API PolynomialPathFitter : public Object { OpenSim_DECLARE_PROPERTY(use_stepwise_regression, bool, "Whether or not to use stepwise regression to fit a minimal set of " "polynomial coefficients."); - OpenSim_DECLARE_PROPERTY(moment_arm_threshold, double, - "The moment arm threshold value that determines whether or not a " - "path depends on a model coordinate. In other words, the moment " - "arm of a path with respect to a coordinate must be greater than " - "this value to be included during path fitting."); OpenSim_DECLARE_PROPERTY(minimum_polynomial_order, int, "The minimum order of the polynomial used to fit each path. The " "order of a polynomial is the highest power of the independent " diff --git a/OpenSim/Simulation/Model/AbstractGeometryPath.h b/OpenSim/Simulation/Model/AbstractGeometryPath.h index dc6274fb93..58044e267b 100644 --- a/OpenSim/Simulation/Model/AbstractGeometryPath.h +++ b/OpenSim/Simulation/Model/AbstractGeometryPath.h @@ -157,6 +157,20 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(AbstractGeometryPath, ModelComponent); */ virtual bool isVisualPath() const = 0; + /** + * Find the list of paths to independent coordinates which fully determine + * the kinematic state of this path. + * + * Internally, this may use a variety of methods to find the list of + * coordinates. It may search the kinematic tree to find the joints lying + * between the origin and insertion points of the path, or it may use + * a user-defined list of coordinates as part of a function-based path + * representation. It is up to concrete implementations + * (e.g., `GeometryPath`) to provide a relevant implementation. + */ + virtual std::vector + findIndependentCoordinatePaths(const SimTK::State&) const = 0; + // DEFAULTED METHODS // // These are methods that for which AbstractGeometryPath provides default diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index ae7ed17e5c..6eb334d62e 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -125,6 +125,12 @@ double FunctionBasedPath::getLength(const SimTK::State& s) const return getCacheVariableValue(s, _lengthCV); } +double FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const +{ + computeLengtheningSpeed(s); + return getCacheVariableValue(s, _lengtheningSpeedCV); +} + double FunctionBasedPath::computeMomentArm(const SimTK::State& s, const Coordinate& coord) const { @@ -150,14 +156,21 @@ void FunctionBasedPath::produceForces(const SimTK::State& state, // Produce mobility forces for (int i = 0; i < (int)_coordinates.size(); ++i) { - forceConsumer.consumeGeneralizedForce(state, *_coordinates[i], momentArms[i] * tension); + forceConsumer.consumeGeneralizedForce( + state, *_coordinates[i], momentArms[i] * tension); } } -double FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const +bool FunctionBasedPath::isVisualPath() const { - computeLengtheningSpeed(s); - return getCacheVariableValue(s, _lengtheningSpeedCV); + return false; +} + +std::vector +FunctionBasedPath:: +findIndependentCoordinatePaths(const SimTK::State& state) const +{ + return getCoordinatePaths(); } //============================================================================= diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h index 13b6b8db25..0941c4caa3 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.h +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -191,7 +191,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPath, AbstractGeometryPath); double tension, ForceConsumer&) const override; - bool isVisualPath() const override { return false; } + bool isVisualPath() const override; + + std::vector + findIndependentCoordinatePaths(const SimTK::State&) const override; private: // MODEL COMPONENT INTERFACE diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 2172e09515..ecdec8d7cb 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -33,6 +33,7 @@ #include #include #include +#include //============================================================================= // STATICS @@ -463,6 +464,35 @@ void GeometryPath::produceForces(const SimTK::State& s, } } +bool GeometryPath::isVisualPath() const +{ + return true; +} + +std::vector +GeometryPath::findIndependentCoordinatePaths(const SimTK::State& s) const { + const PathPointSet& pps = get_PathPointSet(); + const PhysicalFrame& firstFrame = pps.get(0).getParentFrame(); + const PhysicalFrame& lastFrame = pps.get(pps.getSize() - 1).getParentFrame(); + + std::vector> + jointsBetweenFrames = findJointsBetweenPhysicalFrames( + getModel(), + firstFrame.getAbsolutePathString(), + lastFrame.getAbsolutePathString()); + + std::vector coordinatePaths; + for (const auto& joint : jointsBetweenFrames) { + for (int i = 0; i < joint->numCoordinates(); ++i) { + const Coordinate& coord = joint->get_coordinates(i); + if (!coord.isConstrained(s)) { + coordinatePaths.push_back(coord.getAbsolutePathString()); + } + } + } + return coordinatePaths; +} + //_____________________________________________________________________________ /* * Update the geometric representation of the path. diff --git a/OpenSim/Simulation/Model/GeometryPath.h b/OpenSim/Simulation/Model/GeometryPath.h index 05bbc029e1..ea63b58fde 100644 --- a/OpenSim/Simulation/Model/GeometryPath.h +++ b/OpenSim/Simulation/Model/GeometryPath.h @@ -158,9 +158,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, AbstractGeometryPath); void produceForces(const SimTK::State& state, double tension, ForceConsumer& forceConsumer) const override; - - bool isVisualPath() const override { return true; } - + + bool isVisualPath() const override; + + std::vector + findIndependentCoordinatePaths(const SimTK::State&) const override; + //-------------------------------------------------------------------------- // COMPUTATIONS //-------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp b/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp index 8c292b7d9a..b2d2ce9397 100644 --- a/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp +++ b/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include @@ -212,6 +213,37 @@ double Scholz2015GeometryPath::computeMomentArm(const SimTK::State& s, return _maSolver->solve(s, coord, *this); } +bool Scholz2015GeometryPath::isVisualPath() const { + return true; +} + +std::vector +Scholz2015GeometryPath:: +findIndependentCoordinatePaths(const SimTK::State& s) const { + const PhysicalFrame& originFrame = getOrigin().getParentFrame(); + const PhysicalFrame& insertionFrame = getInsertion().getParentFrame(); + + std::vector> + jointsBetweenFrames = findJointsBetweenPhysicalFrames( + getModel(), + originFrame.getAbsolutePathString(), + insertionFrame.getAbsolutePathString()); + + std::vector coordinatePaths; + for (const auto& joint : jointsBetweenFrames) { + for (int i = 0; i < joint->numCoordinates(); ++i) { + const Coordinate& coord = joint->get_coordinates(i); + if (!coord.isConstrained(s)) { + coordinatePaths.push_back(coord.getAbsolutePathString()); + } + } + } + return coordinatePaths; +} + +//============================================================================= +// FORCE PRODUCER INTERFACE +//============================================================================= void Scholz2015GeometryPath::produceForces(const SimTK::State& state, double tension, ForceConsumer& forceConsumer) const { diff --git a/OpenSim/Simulation/Model/Scholz2015GeometryPath.h b/OpenSim/Simulation/Model/Scholz2015GeometryPath.h index 162510a86a..9b8aab4e71 100644 --- a/OpenSim/Simulation/Model/Scholz2015GeometryPath.h +++ b/OpenSim/Simulation/Model/Scholz2015GeometryPath.h @@ -433,7 +433,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPath, AbstractGeometryPath); double getLengtheningSpeed(const SimTK::State& s) const override; double computeMomentArm(const SimTK::State& s, const Coordinate& coord) const override; - bool isVisualPath() const override { return true; } + bool isVisualPath() const override; + + std::vector + findIndependentCoordinatePaths(const SimTK::State&) const override; // @} //** @name `ForceProducer` interface */ diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 6b0adda007..cb86f4fa54 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -546,3 +546,81 @@ void OpenSim::appendCoordinateValueDerivativesAsSpeeds(TimeSeriesTable& table, } } + +std::vector> +OpenSim::findJointsBetweenPhysicalFrames(const Model& model, + const std::string& firstFramePath, const std::string& secondFramePath) { + + // Get the frames from the model. + OPENSIM_THROW_IF( + !model.hasComponent(firstFramePath), Exception, + "Expected the model to contain a PhysicalFrame with path '{}', " + "but no such component was found.", + firstFramePath); + OPENSIM_THROW_IF( + !model.hasComponent(secondFramePath), Exception, + "Expected the model to contain a PhysicalFrame with path '{}', " + "but no such component was found.", + secondFramePath); + const auto& firstFrame = model.getComponent(firstFramePath); + const auto& secondFrame = model.getComponent(secondFramePath); + const std::string firstPath = + firstFrame.findBaseFrame().getAbsolutePathString(); + const std::string secondPath = + secondFrame.findBaseFrame().getAbsolutePathString(); + + // Build a map between the model's joints and the base frames of the joint + // child frames. + std::unordered_map childBodyToJoint; + for (const auto& joint : model.getComponentList()) { + const auto& childBase = joint.getChildFrame().findBaseFrame(); + childBodyToJoint[childBase.getAbsolutePathString()] = &joint; + } + + // A helper function to trace from a starting frame to a target frame, + // populating a list of joints along the way and returning whether the + // target frame was found. If the target frame is not found, the list of + // joints will contain the path from the starting frame to the ground frame. + auto traceToGround = [&](const std::string& startBasePath, + const std::string& targetBasePath, + std::vector>& joints) -> bool { + const Frame* current = &model.getComponent(startBasePath); + while (true) { + if (current->getAbsolutePathString() == targetBasePath) { + return true; + } + auto it = childBodyToJoint.find(current->getAbsolutePathString()); + if (it == childBodyToJoint.end()) break; // reached ground + joints.emplace_back(it->second); + current = &it->second->getParentFrame().findBaseFrame(); + } + return false; + }; + + std::vector> firstJoints; + bool firstFoundTarget = traceToGround(firstPath, secondPath, firstJoints); + if (firstFoundTarget) { + // Reverse the order of the joints so they are returned in root-to-leaf + // order. + std::reverse(firstJoints.begin(), firstJoints.end()); + return firstJoints; + } + + std::vector> secondJoints; + bool secondFoundTarget = traceToGround(secondPath, firstPath, secondJoints); + if (secondFoundTarget) { + // Reverse the order of the joints so they are returned in root-to-leaf + // order. + std::reverse(secondJoints.begin(), secondJoints.end()); + return secondJoints; + } + + // Combine the first list of joints with the reverse of the second list of + // joints. + std::reverse(secondJoints.begin(), secondJoints.end()); + for (const auto& joint : secondJoints) { + firstJoints.emplace_back(joint.get()); + } + + return firstJoints; +} diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 359d34f05b..fd18cde043 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -446,6 +446,23 @@ OSIMSIMULATION_API void appendCoordinateValueDerivativesAsSpeeds( TimeSeriesTable& table, const Model& model, bool overwriteExistingColumns = true); +/// Find the Joint%s that lie between two PhysicalFrame%s in a Model. The second +/// frame need not be a descendant of the first frame or vice versa. If the +/// frames are on different branches of the model, this function will return the +/// list of joints ordered from the first frame to the second frame. Otherwise, +/// the list of joint with be ordered promixally to distally, i.e., moving away +/// from the ground frame. +/// +/// @note This function uses several passes through the model's topology to form +/// the list of joints, so avoided repeated calls to this function in +/// performance critical applications. +/// +/// @ingroup simulationutil +OSIMSIMULATION_API +std::vector> +findJointsBetweenPhysicalFrames(const Model& model, + const std::string& firstFramePath, const std::string& secondFramePath); + } // end of namespace OpenSim diff --git a/OpenSim/Simulation/Test/testSimulationUtilities.cpp b/OpenSim/Simulation/Test/testSimulationUtilities.cpp index 203551a1bc..296fbeeee7 100644 --- a/OpenSim/Simulation/Test/testSimulationUtilities.cpp +++ b/OpenSim/Simulation/Test/testSimulationUtilities.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -171,3 +172,130 @@ TEST_CASE("testUpdatePre40KinematicsFor40MotionType") { Exception); } } + +TEST_CASE("findJointsBetweenPhysicalFrames") { + + using SimTK::Inertia; + using SimTK::Vec3; + + // Create a pendulum model with two branches in the kinematic tree. + Model model; + const auto& ground = model.getGround(); + + auto addBranch = [&ground](Model& model, int numLinks, std::string name) { + const PhysicalFrame* prevBody = &ground; + for (int i = 0; i < numLinks; ++i) { + const std::string istr = std::to_string(i); + auto* bi = new OpenSim::Body(fmt::format("b{}_{}", istr, name), + 1, Vec3(0), Inertia(1)); + model.addBody(bi); + + auto* ji = new PinJoint(fmt::format("j{}_{}", istr, name), + *prevBody, Vec3(0), Vec3(0), + *bi, Vec3(-1, 0, 0), Vec3(0)); + auto& qi = ji->updCoordinate(); + qi.setName(fmt::format("q{}_{}", istr, name)); + model.addJoint(ji); + + prevBody = bi; + } + }; + + addBranch(model, 3, "l"); + addBranch(model, 2, "r"); + model.finalizeConnections(); + + SECTION("Bodies do not exist in the model") { + CHECK_THROWS_WITH( + findJointsBetweenPhysicalFrames( + model, "/not/a/body", "/bodyset/b1_l"), + Catch::Matchers::ContainsSubstring( + "Expected the model to contain a PhysicalFrame with " + "path '/not/a/body'")); + + CHECK_THROWS_WITH( + findJointsBetweenPhysicalFrames( + model, "/bodyset/b0_l", "/also/not/a/body"), + Catch::Matchers::ContainsSubstring( + "Expected the model to contain a PhysicalFrame with " + "path '/also/not/a/body'")); + } + + SECTION("Frames on same branch") { + auto joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b0_l", "/bodyset/b1_l"); + CHECK(joints.size() == 1); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j1_l"); + + // Joints should be returned in root-to-leaf order. + joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b0_l", "/bodyset/b2_l"); + CHECK(joints.size() == 2); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j1_l"); + CHECK(joints[1]->getAbsolutePathString() == "/jointset/j2_l"); + + // Flipping the order of the frames should produce the same joints. + joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b2_l", "/bodyset/b0_l"); + CHECK(joints.size() == 2); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j1_l"); + CHECK(joints[1]->getAbsolutePathString() == "/jointset/j2_l"); + + joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b1_l", "/bodyset/b2_l"); + CHECK(joints.size() == 1); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j2_l"); + + joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b0_r", "/bodyset/b1_r"); + CHECK(joints.size() == 1); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j1_r"); + } + + SECTION("Frames on different branches") { + auto joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b0_l", "/bodyset/b0_r"); + CHECK(joints.size() == 2); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j0_l"); + CHECK(joints[1]->getAbsolutePathString() == "/jointset/j0_r"); + + joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b0_r", "/bodyset/b0_l"); + CHECK(joints.size() == 2); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j0_r"); + CHECK(joints[1]->getAbsolutePathString() == "/jointset/j0_l"); + + joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b2_l", "/bodyset/b1_r"); + CHECK(joints.size() == 5); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j2_l"); + CHECK(joints[1]->getAbsolutePathString() == "/jointset/j1_l"); + CHECK(joints[2]->getAbsolutePathString() == "/jointset/j0_l"); + CHECK(joints[3]->getAbsolutePathString() == "/jointset/j0_r"); + CHECK(joints[4]->getAbsolutePathString() == "/jointset/j1_r"); + + joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b1_r", "/bodyset/b2_l"); + CHECK(joints.size() == 5); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j1_r"); + CHECK(joints[1]->getAbsolutePathString() == "/jointset/j0_r"); + CHECK(joints[2]->getAbsolutePathString() == "/jointset/j0_l"); + CHECK(joints[3]->getAbsolutePathString() == "/jointset/j1_l"); + CHECK(joints[4]->getAbsolutePathString() == "/jointset/j2_l"); + } + + SECTION("One frame is ground") { + auto joints = findJointsBetweenPhysicalFrames( + model, "/ground", "/bodyset/b2_l"); + CHECK(joints.size() == 3); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j0_l"); + CHECK(joints[1]->getAbsolutePathString() == "/jointset/j1_l"); + CHECK(joints[2]->getAbsolutePathString() == "/jointset/j2_l"); + + joints = findJointsBetweenPhysicalFrames( + model, "/bodyset/b1_r", "/ground"); + CHECK(joints.size() == 2); + CHECK(joints[0]->getAbsolutePathString() == "/jointset/j0_r"); + CHECK(joints[1]->getAbsolutePathString() == "/jointset/j1_r"); + } +} diff --git a/OpenSim/Tests/Wrapping/testGeometryPathWrapping.cpp b/OpenSim/Tests/Wrapping/testGeometryPathWrapping.cpp index 63c5b577e1..c4b4f6c0b9 100644 --- a/OpenSim/Tests/Wrapping/testGeometryPathWrapping.cpp +++ b/OpenSim/Tests/Wrapping/testGeometryPathWrapping.cpp @@ -459,3 +459,55 @@ TEST_CASE("WrapEllipsoid") { } } } + +TEST_CASE("findCoordinatesBetween") { + Model model("subject_walk_armless_18musc.osim"); + SimTK::State state = model.initSystem(); + + SECTION("hip muscles") { + const std::string muscle_name = GENERATE("psoas_r", "glut_max2_r"); + const std::string muscle_path = fmt::format("/forceset/{}", muscle_name); + const auto& path = model.getComponent(muscle_path).getPath(); + auto coordinates = path.findIndependentCoordinatePaths(state); + CHECK(coordinates[0] == "/jointset/hip_r/hip_flexion_r"); + CHECK(coordinates[1] == "/jointset/hip_r/hip_adduction_r"); + CHECK(coordinates[2] == "/jointset/hip_r/hip_rotation_r"); + } + + SECTION("hip/knee biarticular muscles") { + const std::string muscle_name = GENERATE("rect_fem_r", "semimem_r"); + const std::string muscle_path = fmt::format("/forceset/{}", muscle_name); + const auto& muscle = model.getComponent(muscle_path); + const auto& path = model.getComponent(muscle_path).getPath(); + auto coordinates = path.findIndependentCoordinatePaths(state); + CHECK(coordinates[0] == "/jointset/hip_r/hip_flexion_r"); + CHECK(coordinates[1] == "/jointset/hip_r/hip_adduction_r"); + CHECK(coordinates[2] == "/jointset/hip_r/hip_rotation_r"); + CHECK(coordinates[3] == "/jointset/walker_knee_r/knee_angle_r"); + } + + SECTION("knee muscles") { + const std::string muscle_name = GENERATE("vas_int_r", "bifemsh_r"); + const std::string muscle_path = fmt::format("/forceset/{}", muscle_name); + const auto& muscle = model.getComponent(muscle_path); + const auto& path = model.getComponent(muscle_path).getPath(); + auto coordinates = path.findIndependentCoordinatePaths(state); + CHECK(coordinates[0] == "/jointset/walker_knee_r/knee_angle_r"); + } + + SECTION("gastrocnemius") { + const std::string muscle_path = "/forceset/med_gas_r"; + const auto& path = model.getComponent(muscle_path).getPath(); + auto coordinates = path.findIndependentCoordinatePaths(state); + CHECK(coordinates[0] == "/jointset/walker_knee_r/knee_angle_r"); + CHECK(coordinates[1] == "/jointset/ankle_r/ankle_angle_r"); + } + + SECTION("ankle muscles") { + const std::string muscle_name = GENERATE("tib_ant_r", "soleus_r"); + const std::string muscle_path = fmt::format("/forceset/{}", muscle_name); + const auto& path = model.getComponent(muscle_path).getPath(); + auto coordinates = path.findIndependentCoordinatePaths(state); + CHECK(coordinates[0] == "/jointset/ankle_r/ankle_angle_r"); + } +}