[WIP] Use topology-based moment arm detection in PolynomialPathFitter

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

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();
     }
 
+    /// <b>(Deprecated)</b> 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) {}
+    /// <b>(Deprecated)</b> 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 "

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<std::string>
+    findIndependentCoordinatePaths(const SimTK::State&) const = 0;
+
     // DEFAULTED METHODS
     //
     // These are methods that for which AbstractGeometryPath provides default

OpenSim/Simulation/Model/FunctionBasedPath.cpp

@@ -125,6 +125,12 @@ double FunctionBasedPath::getLength(const SimTK::State& s) const
     return getCacheVariableValue<double>(s, _lengthCV);
 }
 
+double FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const
+{
+    computeLengtheningSpeed(s);
+    return getCacheVariableValue<double>(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<double>(s, _lengtheningSpeedCV);
+    return false;
+}
+
+std::vector<std::string>
+FunctionBasedPath::
+findIndependentCoordinatePaths(const SimTK::State& state) const
+{
+    return getCoordinatePaths();
 }
 
 //=============================================================================

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<std::string>
+    findIndependentCoordinatePaths(const SimTK::State&) const override;
 
 private:
     // MODEL COMPONENT INTERFACE

OpenSim/Simulation/Model/GeometryPath.cpp

@@ -33,6 +33,7 @@
 #include <OpenSim/Simulation/Model/MovingPathPoint.h>
 #include <OpenSim/Simulation/Model/PointForceDirection.h>
 #include <OpenSim/Simulation/Wrap/PathWrap.h>
+#include <OpenSim/Simulation/SimulationUtilities.h>
 
 //=============================================================================
 // STATICS
@@ -463,6 +464,35 @@ void GeometryPath::produceForces(const SimTK::State& s,
     }
 }
 
+bool GeometryPath::isVisualPath() const
+{
+    return true;
+}
+
+std::vector<std::string>
+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<SimTK::ReferencePtr<const Joint>>
+    jointsBetweenFrames = findJointsBetweenPhysicalFrames(
+            getModel(),
+            firstFrame.getAbsolutePathString(),
+            lastFrame.getAbsolutePathString());
+
+    std::vector<std::string> 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.

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<std::string>
+    findIndependentCoordinatePaths(const SimTK::State&) const override;
+
     //--------------------------------------------------------------------------
     // COMPUTATIONS
     //--------------------------------------------------------------------------

OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp

@@ -27,6 +27,7 @@
 #include <OpenSim/Simulation/Model/ForceConsumer.h>
 #include <OpenSim/Simulation/SimbodyEngine/Coordinate.h>
 #include <OpenSim/Simulation/Model/Model.h>
+#include <OpenSim/Simulation/SimulationUtilities.h>
 
 #include <optional>
 
@@ -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<std::string>
+Scholz2015GeometryPath::
+findIndependentCoordinatePaths(const SimTK::State& s) const {
+    const PhysicalFrame& originFrame = getOrigin().getParentFrame();
+    const PhysicalFrame& insertionFrame = getInsertion().getParentFrame();
+
+    std::vector<SimTK::ReferencePtr<const Joint>>
+    jointsBetweenFrames = findJointsBetweenPhysicalFrames(
+            getModel(),
+            originFrame.getAbsolutePathString(),
+            insertionFrame.getAbsolutePathString());
+
+    std::vector<std::string> 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 {
 

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<std::string>
+    findIndependentCoordinatePaths(const SimTK::State&) const override;
     // @}
 
     //** @name `ForceProducer` interface */

OpenSim/Simulation/SimulationUtilities.cpp

@@ -546,3 +546,81 @@ void OpenSim::appendCoordinateValueDerivativesAsSpeeds(TimeSeriesTable& table,
     }
 
 }
+
+std::vector<SimTK::ReferencePtr<const Joint>>
+OpenSim::findJointsBetweenPhysicalFrames(const Model& model,
+        const std::string& firstFramePath, const std::string& secondFramePath) {
+
+    // Get the frames from the model.
+    OPENSIM_THROW_IF(
+            !model.hasComponent<PhysicalFrame>(firstFramePath), Exception,
+            "Expected the model to contain a PhysicalFrame with path '{}', "
+            "but no such component was found.",
+            firstFramePath);
+    OPENSIM_THROW_IF(
+            !model.hasComponent<PhysicalFrame>(secondFramePath), Exception,
+            "Expected the model to contain a PhysicalFrame with path '{}', "
+            "but no such component was found.",
+            secondFramePath);
+    const auto& firstFrame = model.getComponent<PhysicalFrame>(firstFramePath);
+    const auto& secondFrame = model.getComponent<PhysicalFrame>(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<std::string, const Joint*> childBodyToJoint;
+    for (const auto& joint : model.getComponentList<Joint>()) {
+        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<SimTK::ReferencePtr<const Joint>>& joints) -> bool {
+        const Frame* current = &model.getComponent<PhysicalFrame>(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<SimTK::ReferencePtr<const Joint>> 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<SimTK::ReferencePtr<const Joint>> 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;
+}