Fix IDTool and IDSolver to account for extra q's in state (#2971)

* add test with a model with extra q slot (ThorascapularShoulderModel)

* fix new test in testID. allow InverseDynamicsSolver to handle model with different number of q's and u's. fix InverseDyanmicsTool for models with extra (unused) q slots

* update CHANGELOG.md for ID fix

* int -> size_t

* size_t -> int

* one more size_t -> int

* stack allocate coordinate functions

* address comments from review by @aymanhab

* add thoracoscapularmodel from shared folder to ID test

* also account for if u's are in a different order (than q's or coordinate's order). covers when qdot != u.

* size_t -> int

* add testBallJoint for accounting for u and udot order in ID

* variable renaming and documentation cleanup

* also test that ID will throw if incorrect udot is used

* one more documentation cleanup

* Replace one letter variable names

* Use log_cout and formatting instead of std::cout, also disable testBallJoint as flaky due to issues unrelated to the test contents

* Clarify comments and language

* Use const reference instead of an actual std::vector/copy as argument for the new method

Co-authored-by: carmichaelong <carmichaelong@gmail.com>
Co-authored-by: aymanhab <ahabib@stanford.edu>

Applications/ID/test/CMakeLists.txt

@@ -1,6 +1,7 @@
 
 file(GLOB TEST_PROGS "test*.cpp")
 file(GLOB TEST_FILES *.osim *.xml *.sto *.mot *.trc)
+list(APPEND TEST_FILES ${CMAKE_SOURCE_DIR}/OpenSim/Tests/shared/ThoracoscapularShoulderModel.osim)
 
 OpenSimAddTests(
     TESTPROGRAMS ${TEST_PROGS}

Applications/ID/test/ThorascapularShoulderModel_ID_static.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<OpenSimDocument Version="40000">
+	<InverseDynamicsTool name="DummyModel">
+		<!--Name of the directory where results are written. Be default this is the directory in which the setup file is be  executed.-->
+		<results_directory></results_directory>
+		<!--Name of the .osim file used to construct a model.-->
+		<model_file>ThoracoscapularShoulderModel.osim</model_file>
+		<!--Time range over which the inverse dynamics problem is solved.-->
+		<time_range> 0.01 0.06</time_range>
+		<!--List of forces by individual or grouping name (e.g. All, actuators, muscles, ...) to be excluded when computing model dynamics. 'All' also excludes external loads added via 'external_loads_file'.-->
+		<forces_to_exclude> Muscles</forces_to_exclude>
+		<!--XML file (.xml) containing the external loads applied to the model as a set of ExternalForce(s).-->
+		<external_loads_file />
+		<!--The name of the file containing coordinate data. Can be a motion (.mot) or a states (.sto) file.-->
+		<coordinates_file>ThorascapularShoulderModel_static.mot</coordinates_file>
+		<!--Low-pass cut-off frequency for filtering the coordinates_file data (currently does not apply to states_file or speeds_file). A negative value results in no filtering. The default value is -1.0, so no filtering.-->
+		<lowpass_cutoff_frequency_for_coordinates>-1</lowpass_cutoff_frequency_for_coordinates>
+		<!--Name of the storage file (.sto) to which the generalized forces are written.-->
+		<output_gen_force_file>ThorascapularShoulderModel_ID_output.sto</output_gen_force_file>
+		<!--List of joints (keyword All, for all joints) to report body forces acting at the joint frame expressed in ground.-->
+		<joints_to_report_body_forces />
+		<!--Name of the storage file (.sto) to which the body forces at specified joints are written.-->
+		<output_body_forces_file />
+	</InverseDynamicsTool>
+</OpenSimDocument>

Applications/ID/test/ThorascapularShoulderModel_static.mot

@@ -0,0 +1,13 @@
+Default_Position.mot																	
+version=1																	
+nRows=6																	
+nColumns=18																	
+inDegrees=no																	
+endheader																	
+time	ground_thorax_rot_x	ground_thorax_rot_y	ground_thorax_rot_z	ground_thorax_tx	ground_thorax_ty	ground_thorax_tz	clav_prot	clav_elev	scapula_abduction	scapula_elevation	scapula_upward_rot	scapula_winging	plane_elv	shoulder_elv	axial_rot	elbow_flexion	pro_sup
+0.01	0.01	0.02	0.03	0.04	0.05	0.06	0.07	0.08	0.09	0.1	0.11	0.12	0.13	0.14	0.15	0.16	0.17
+0.02	0.01	0.02	0.03	0.04	0.05	0.06	0.07	0.08	0.09	0.1	0.11	0.12	0.13	0.14	0.15	0.16	0.17
+0.03	0.01	0.02	0.03	0.04	0.05	0.06	0.07	0.08	0.09	0.1	0.11	0.12	0.13	0.14	0.15	0.16	0.17
+0.04	0.01	0.02	0.03	0.04	0.05	0.06	0.07	0.08	0.09	0.1	0.11	0.12	0.13	0.14	0.15	0.16	0.17
+0.05	0.01	0.02	0.03	0.04	0.05	0.06	0.07	0.08	0.09	0.1	0.11	0.12	0.13	0.14	0.15	0.16	0.17
+0.06	0.01	0.02	0.03	0.04	0.05	0.06	0.07	0.08	0.09	0.1	0.11	0.12	0.13	0.14	0.15	0.16	0.17

Applications/ID/test/testID.cpp

@@ -28,11 +28,18 @@
 #include <OpenSim/Simulation/Model/Model.h>
 #include <OpenSim/Simulation/Model/ForceSet.h>
 #include <OpenSim/Tools/InverseDynamicsTool.h>
+#include <OpenSim/Simulation/InverseDynamicsSolver.h>
 #include <OpenSim/Auxiliary/auxiliaryTestFunctions.h>
+#include <OpenSim/Simulation/SimbodyEngine/BallJoint.h>
+#include <OpenSim/Simulation/SimulationUtilities.h>
+#include <OpenSim/Common/GCVSplineSet.h>
 
 using namespace OpenSim;
 using namespace std;
 
+void testThoracoscapularShoulderModel();
+void testBallJoint();
+
 int main()
 {
     try {
@@ -51,6 +58,13 @@ int main()
             std::vector<double>(23, 2.0), __FILE__, __LINE__,
             "testGait failed");
         cout << "testGait passed" << endl;
+
+        testThoracoscapularShoulderModel();
+        cout << "testThoracoscapularShoulderModel passed" << endl;
+        // Commented out testBallJoint due to sporadic crash in Model destructor
+        // -Ayman 03/21
+        //testBallJoint();
+        //cout << "testBallJoint passed" << endl;
     }
     catch (const Exception& e) {
         e.print(cerr);
@@ -59,3 +73,149 @@ int main()
     cout << "Done" << endl;
     return 0;
 }
+
+void testThoracoscapularShoulderModel() {
+    // Perform ID by setting coordinate values and setting up an
+    // InverseDynamicsSovler directly
+    Model model("ThoracoscapularShoulderModel.osim");
+    SimTK::State& s = model.initSystem();
+    TimeSeriesTable motionTable("ThorascapularShoulderModel_static.mot");
+    auto labels = motionTable.getColumnLabels();
+
+    for (size_t i = 0; i < labels.size(); ++i) {
+        const Coordinate& thisCoord = model.getCoordinateSet().get(labels[i]);
+        auto thisValue = motionTable.getDependentColumn(labels[i])[0];
+        log_cout("ThoracoscapularShoulderModel set {} to {}", labels[i], thisValue);
+        thisCoord.setLocked(s, false);
+        thisCoord.setValue(s, thisValue, false);
+        thisCoord.setSpeedValue(s, 0);
+    }
+    // IDTool and IDSolver specifically do not perform assembly since it 
+    // updates the state directly, so don't assemble here either
+    //model.assemble(s);
+
+    InverseDynamicsSolver idSolver(model);
+    Set<Muscle>& muscles = model.updMuscles();
+    for (int i = 0; i < muscles.getSize(); i++) {
+        muscles[i].setAppliesForce(s, false);
+    }
+
+    SimTK::Vector idSolverVec = idSolver.solve(s);
+    auto coordsInMultibodyOrder = model.getCoordinatesInMultibodyTreeOrder();
+    int nCoords = (int)coordsInMultibodyOrder.size();
+
+    // Do the same thing with InverseDynamicsTool
+    InverseDynamicsTool idTool("ThorascapularShoulderModel_ID_static.xml");
+    idTool.run();
+    TimeSeriesTable idToolTable("ThorascapularShoulderModel_ID_output.sto");
+    auto idToolLabels = idToolTable.getColumnLabels();
+    auto row = idToolTable.getRowAtIndex(0);
+    SimTK::Vector idToolVec(nCoords);
+
+    // Reorder Storage file results to multibody tree order just in case
+    for (int i = 0; i < nCoords; ++i) {
+        std::string genForce = coordsInMultibodyOrder[i]->getName();
+        genForce += (coordsInMultibodyOrder[i]->getMotionType() ==
+                     Coordinate::Rotational) ? "_moment" : "_force";
+
+        int colInd = (int)idToolTable.getColumnIndex(genForce);
+        idToolVec[i] = row[colInd];
+    }
+
+    // Compare results
+    ASSERT_EQUAL(idSolverVec, idToolVec, 1e-6, __FILE__, __LINE__, 
+        "testThoracoscapularShoulderModel failed");
+}
+
+void testBallJoint() {
+    Model mdl;
+    Body* bdy = new Body("body", 1.0, SimTK::Vec3(0), SimTK::Inertia(1));
+    BallJoint* jnt = new BallJoint("joint", mdl.getGround(), *bdy);
+    mdl.addBody(bdy);
+    mdl.addJoint(jnt);
+    jnt->upd_coordinates(0).setName("c0");
+    jnt->upd_coordinates(1).setName("c1");
+    jnt->upd_coordinates(2).setName("c2");
+
+    SimTK::State& s = mdl.initSystem();
+
+    // only one joint, coord order is c0, c1, c2
+    auto coords = mdl.getCoordinatesInMultibodyTreeOrder();
+
+    // BallJoint is backed by a quaternion.
+    // q has length of 4: c0, c1, c2, (unused)
+    // u has length of 3: c0, c1, c2
+    // state has length 7: [q, u]
+    auto coordMap = createSystemYIndexMap(mdl);
+    for (const auto& c : coordMap) {
+        log_cout("Map entry {} to index {}", c.first,  c.second);
+    }
+
+    Array<string> coordStorageLabels;
+    coordStorageLabels.append("time");
+    for (const auto& c : coords) { 
+        coordStorageLabels.append(c->getName()); // order is c0, c1, c2
+        log_cout("{}", c->getName());
+    }
+
+    Storage coordStorage;
+    coordStorage.setColumnLabels(coordStorageLabels);
+    for (int i = 0; i < 10; ++i) {
+        double vals[3] = {0.1*pow(2, i), 0.2*pow(2, i), 0.3*pow(2, i)};
+        SimTK::Vector q(3, vals);
+        coordStorage.append(0.1*i, q);
+    }
+    coordStorage.print("testBallJoint.sto");
+    Array<double> timeVec;
+    coordStorage.getTimeColumn(timeVec);
+    log_cout("{}", timeVec);
+
+    // Setup IDSolver 
+    double analyzeTime = 0.5;
+    SimTK::Vector udot(mdl.getNumCoordinates());
+    GCVSplineSet coordSplines(5, &coordStorage);
+    for (int i = 0; i < mdl.getNumCoordinates(); ++i) {
+        jnt->upd_coordinates(i).setValue(
+                s, coordSplines.evaluate(i, 0, analyzeTime));
+        jnt->upd_coordinates(i).setSpeedValue(
+                s, coordSplines.evaluate(i, 1, analyzeTime));
+        udot[i] = coordSplines.evaluate(i, 2, analyzeTime);
+    }
+    log_cout("{}", s.getQ());
+    log_cout("{}", s.getU());
+    log_cout("{}", udot);
+
+    InverseDynamicsSolver idSolver(mdl);
+    Set<Muscle>& muscles = mdl.updMuscles();
+    for (int i = 0; i < muscles.getSize(); i++) {
+        muscles[i].setAppliesForce(s, false);
+    }
+    SimTK::Vector idSolverVecZeroUDot = idSolver.solve(s);
+    SimTK::Vector idSolverVec = idSolver.solve(s, udot);
+
+    // Compare with IDTool
+    InverseDynamicsTool idTool;
+    string outputFile = "testBallJoint_ID_output.sto";
+    idTool.setModel(mdl);
+    idTool.setCoordinateValues(coordStorage);
+    idTool.setOutputGenForceFileName(outputFile);
+    idTool.run();
+
+    // Compare results (should be in same order already)
+    Storage idToolOutput(outputFile);
+    Array<double> idToolArray;
+    idToolOutput.getDataAtTime(analyzeTime, mdl.getNumCoordinates(), idToolArray);
+    SimTK::Vector idToolVec(mdl.getNumCoordinates());
+    for (int i = 0; i < mdl.getNumCoordinates(); ++i) {
+        idToolVec[i] = idToolArray[i];
+    }
+
+    // Test should pass when correct udot is used
+    ASSERT_EQUAL(idSolverVec, idToolVec, 1e-6, __FILE__, __LINE__,
+            "testThoracoscapularShoulderModel failed");
+
+    // Test should not pass when default udot = 0 is used instead
+    ASSERT_THROW(Exception,
+            ASSERT_EQUAL(idSolverVecZeroUDot, idToolVec, 1e-6, 
+            __FILE__, __LINE__, "testThoracoscapularShoulderModel failed"));
+}

CHANGELOG.md

@@ -8,6 +8,7 @@ This is not a comprehensive list of changes but rather a hand-curated collection
 
 v4.2
 ====
+- Fixed a bug with InverseDynamicsTool/InverseDynamicsSolver to account for cases where a model has extra slots in its `State`'s "q" (PR #2971)
 - Added Bhargava2004SmoothedMuscleMetabolics, a smoothed version of the Bhargava metabolics model designed for gradient-based optimization (i.e., Moco).
 - Fixed a bug in Millard2012EquilibriumMuscle::extendFinalizeFromProperties(): the end point slopes on the inverse force velocity curves are constrained to yield a valid curve. A warning is noted in the log if the slopes are small enough that numerical integration might be slow.
 - Added logging to Millard2012EquilibriumMuscle::extendFinalizeFromProperties(): whenever an internal setting is changed automatically these changes are noted in the log. To avoid seeing these messages, update the corresponding properties in the .osim file to the values noted in the log message.

OpenSim/Simulation/InverseDynamicsSolver.cpp

@@ -96,14 +96,16 @@ Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector &
     state variables (like muscle fiber lengths) that are not known */
 Vector InverseDynamicsSolver::solve(SimTK::State &s, const FunctionSet &Qs, double time)
 {
-    int nq = getModel().getNumCoordinates();
+    int nq = s.getNQ();
+    int nu = s.getNU();
 
-    if(Qs.getSize() != nq){
+    if (Qs.getSize() != nq) {
         throw Exception("InverseDynamicsSolver::solve invalid number of q functions.");
     }
 
-    if( nq != getModel().getNumSpeeds()){
-        throw Exception("InverseDynamicsSolver::solve using FunctionSet, nq != nu not supported.");
+    if (nq != nu) {
+        throw Exception("InverseDynamicsSolver::solve using only FunctionSet of "
+                        "qs, nq != nu not supported.");
     }
 
     // update the State so we get the correct gravity and Coriolis effects
@@ -123,15 +125,47 @@ Vector InverseDynamicsSolver::solve(SimTK::State &s, const FunctionSet &Qs, doub
     return solve(s, udot);
 }
 
+Vector InverseDynamicsSolver::solve(SimTK::State& s, const FunctionSet& Qs, 
+        const std::vector<int>& coordinatesToSpeedsIndexMap, double time) {
+    int nq = s.getNQ();
+    int nu = s.getNU();
+
+    if (Qs.getSize() != nq) {
+        throw Exception("InverseDynamicsSolver::solve invalid number of q functions.");
+    }
+
+    if ((int)coordinatesToSpeedsIndexMap.size() != nu) {
+        throw Exception("InverseDynamicsSolver::solve coordinatesToSpeedsIndexMap must be 'nu' long");
+    }
+
+    // update the State so we get the correct gravity and Coriolis effects
+    // direct references into the state so no allocation required
+    s.updTime() = time;
+    Vector& q = s.updQ();
+    Vector& u = s.updU();
+    Vector& udot = s.updUDot();
+
+    for (int i = 0; i < nq; i++) {
+        q[i] = Qs.evaluate(i, 0, time);
+    }
+
+    for (int i = 0; i < nu; i++) {
+        u[i] = Qs.evaluate(coordinatesToSpeedsIndexMap[i], 1, time);
+        udot[i] = Qs.evaluate(coordinatesToSpeedsIndexMap[i], 2, time);
+    }
+
+    // Perform general inverse dynamics
+    return solve(s, udot);
+}
 
 /** Same as above but for a given time series */
 void InverseDynamicsSolver::solve(SimTK::State &s, const FunctionSet &Qs, const Array_<double> &times, Array_<Vector> &genForceTrajectory)
 {
-    int nq = getModel().getNumCoordinates();
+    int nCoords = getModel().getNumCoordinates();
     int nt = times.size();
 
     //Preallocate if not done already
-    genForceTrajectory.resize(nt, Vector(nq));
+    genForceTrajectory.resize(nt, Vector(nCoords));
 
     AnalysisSet& analysisSet = const_cast<AnalysisSet&>(getModel().getAnalysisSet());
     //fill in results for each time
@@ -141,4 +175,24 @@ void InverseDynamicsSolver::solve(SimTK::State &s, const FunctionSet &Qs, const
     }
 }
 
+void InverseDynamicsSolver::solve(SimTK::State& s, const FunctionSet& Qs,
+        const std::vector<int> coordinatesToSpeedsIndexMap,
+        const Array_<double>& times,
+        Array_<Vector>& genForceTrajectory) {
+    int nCoords = getModel().getNumCoordinates();
+    int nt = times.size();
+
+    // Preallocate if not done already
+    genForceTrajectory.resize(nt, Vector(nCoords));
+
+    AnalysisSet& analysisSet =
+            const_cast<AnalysisSet&>(getModel().getAnalysisSet());
+    // fill in results for each time
+    for (int i = 0; i < nt; i++) {
+        genForceTrajectory[i] =
+                solve(s, Qs, coordinatesToSpeedsIndexMap, times[i]);
+        analysisSet.step(s, i);
+    }
+}
+
 } // end of namespace OpenSim

OpenSim/Simulation/InverseDynamicsSolver.h

@@ -88,16 +88,39 @@ OpenSim_DECLARE_CONCRETE_OBJECT(InverseDynamicsSolver, Solver);
     /** Solve the inverse dynamics system of equations for generalized coordinate 
         forces, Tau. Now the state is updated from known coordinates, q, as 
         functions of time. Coordinate functions must be twice differentiable and 
-        are used to supply the coordinate speed and acceleration
+        are used to supply the coordinate speed and acceleration. Coordinate
+        functions must be in the same order as the order of q's, u's, and udot's
+        in the provided SimTK::State.
         NOTE: forces with internal states should be removed/disabled prior to  
               solving if default state is inappropriate */
     virtual SimTK::Vector solve(SimTK::State& s, const FunctionSet& Qs, double time);
+
+    /** This is the same as above, but can be used when qdot != u. This adds an
+        extra vector, coordinatesToSpeedsIndexMap, which is the length of number of u's in
+        the SimTK::State, and whose i'th index is the index of the FunctionSet
+        Qs from which each 'u' and 'udot' will be calculated. */
+    virtual SimTK::Vector solve(SimTK::State& s, const FunctionSet& Qs, 
+                                const std::vector<int>& coordinatesToSpeedsIndexMap,
+                                double time);
+
 #ifndef SWIG
-    /** Same as above but for a given time series populate an Array (trajectory) of
-        generalized-coordinate forces (Vector) */
+    /** Same as above but for a given time series populate an Array (trajectory)
+        of generalized-coordinate forces (Vector). Coordinate functions must be
+        in the same order as the order of q's, u's, and udot's in the provided
+        SimTK::State. */
     virtual void solve(SimTK::State& s, const FunctionSet& Qs, 
-                 const SimTK::Array_<double>&  times,
+                 const SimTK::Array_<double>& times,
                  SimTK::Array_<SimTK::Vector>& genForceTrajectory);
+
+    /** Same as above but for a given time series populate an Array (trajectory)
+       of generalized-coordinate forces (Vector) that can be used when qdot != u.
+       This adds an extra vector, coordinatesToSpeedsIndexMap, which is the length of number of
+       u's in the SimTK::State, and whose i'th index is the index of the
+       FunctionSet Qs from which each 'u' and 'udot' will be calculated. */ 
+    virtual void solve(SimTK::State& s, const FunctionSet& Qs, 
+            const std::vector<int> coordinatesToSpeedsIndexMap,
+            const SimTK::Array_<double>& times,
+            SimTK::Array_<SimTK::Vector>& genForceTrajectory);
 #endif
 //=============================================================================
 };  // END of class InverseDynamicsSolver