Merge pull request #117 from keenon/linear_map_multiple_trials_fix

Bug fixes and improvements to `DynamicsFitter` and `OpenSimParser`

VERSION.txt

@@ -1 +1 @@
-0.8.79
+0.8.80

dart/biomechanics/DynamicsFitter.cpp

@@ -10254,6 +10254,7 @@ void DynamicsFitter::moveComsToMinimizeAngularResiduals(
 // the current kinematic fit.
 void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
     std::shared_ptr<DynamicsInitialization> init,
+    int maxTrialsToSolveMassOver,
     bool detectExternalForce,
     int driftCorrectionBlurRadius,
     int driftCorrectionBlurInterval)
@@ -10278,6 +10279,10 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
   ResidualForceHelper helper(mSkeleton, init->grfBodyIndices);
 #endif
 
+  int numTrials = init->poseTrials.size();
+  int numTrialsToSolveMassOver = numTrials < maxTrialsToSolveMassOver ?
+          numTrials : maxTrialsToSolveMassOver;
+
   std::vector<std::vector<Eigen::Vector3s>> trialGRFs;
   std::vector<std::vector<Eigen::Vector3s>> trialOriginalCOMs;
   std::vector<std::vector<Eigen::Vector3s>> trialOriginalCenteredCOMs;
@@ -10340,8 +10345,6 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
     std::vector<Eigen::VectorXs> trialOriginalPositions;
     std::vector<Eigen::VectorXs> trialOriginalGravityOffsets;
 
-    const int maxTrajectoriesToSolveMassOver = 1;
-
     int totalSampledColsWithoutMass = 0;
     int totalSampledRows = 0;
 
@@ -10391,11 +10394,14 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
 
       // This has one col each for start COM positions, start COM velocities,
       // and total mass. It outputs the physically consistent X, Y, Z
-      // coordinates of COM over time, given those inputs.
+      // coordinates of COM over time, given those inputs. Additionally, there
+      // are columns to account for small force plate rotation errors (3 angles
+      // per force plate) and for instantaneous COM velocities at time steps with
+      // missing ground reaction force data.
       Eigen::MatrixXs trialLinearMapToPositions = Eigen::MatrixXs::Zero(
           numTimesteps * 3 + (numForcePlates * 3) + (numMissingSteps * 3),
           6 + (numForcePlates * 3) + (numMissingSteps * 3) + 1);
-      if (i < maxTrajectoriesToSolveMassOver)
+      if (i < maxTrialsToSolveMassOver)
       {
         totalSampledColsWithoutMass += trialLinearMapToPositions.cols() - 1;
         totalSampledRows += trialLinearMapToPositions.rows();
@@ -10579,7 +10585,6 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
       trialOriginalPositions.push_back(originalCOMPositions);
       trialOriginalGravityOffsets.push_back(comGravityOffset);
     }
-    int numTrials = trialLinearMaps.size();
 
     std::cout << "Assembling the unified linear COM trajectory map across "
               << numTrials << " trials" << std::endl;
@@ -10589,7 +10594,6 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
     // trial separate, but collapses each trial's mass quantity into a single
     // column, so that we can solve them together for a single unified
     // skeleton mass that is least-squares best across all the trials at once.
-
     Eigen::MatrixXs unifiedLinearMap = Eigen::MatrixXs::Zero(
         totalSampledRows, totalSampledColsWithoutMass + 1);
     Eigen::VectorXs unifiedPositions = Eigen::VectorXs::Zero(totalSampledRows);
@@ -10600,8 +10604,7 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
     int colCursor = 0;
     int massCol = unifiedLinearMap.cols() - 1;
     for (int trial = 0;
-         trial < numTrials && trial < maxTrajectoriesToSolveMassOver;
-         trial++)
+         trial < numTrials && trial < maxTrialsToSolveMassOver; trial++)
     {
       int trialTimestepRows = trialOriginalPositions[trial].size();
       int trialRows = trialLinearMaps[trial].rows();
@@ -10622,8 +10625,8 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
     }
     assert(colCursor == totalSampledColsWithoutMass);
 
-    std::cout << "Solving linear COM trajectory map: size="
-              << unifiedLinearMap.rows() << "x" << unifiedLinearMap.cols()
+    std::cout << "Solving unified linear COM trajectory map: size = "
+              << unifiedLinearMap.rows() << " x " << unifiedLinearMap.cols()
               << std::endl;
 
     // Now that we've formulated our problem, we can attempt to solve:
@@ -10633,7 +10636,7 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
     //     unifiedPositions - unifiedGravityOffset);
     Eigen::LeastSquaresConjugateGradient<Eigen::MatrixXs> solver;
     solver.setTolerance(1e-9);
-    solver.setMaxIterations(200);
+    solver.setMaxIterations(200 * numTrialsToSolveMassOver);
     solver.compute(unifiedLinearMap);
     Eigen::VectorXs tentativeResult
         = solver.solve(unifiedPositions - unifiedGravityOffset);
@@ -10696,7 +10699,7 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
     rowCursor = 0;
     for (int trial = 0; trial < numTrials; trial++)
     {
-      if (trial < maxTrajectoriesToSolveMassOver)
+      if (trial < maxTrialsToSolveMassOver)
       {
         int trialCols = trialLinearMaps[trial].cols();
         int trialRows = trialLinearMaps[trial].rows();
@@ -10724,8 +10727,9 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
         bFixedMass.segment(0, b.size()) += b;
 
         Eigen::VectorXs desired = Eigen::VectorXs::Zero(bFixedMass.size());
-        desired.segment(0, trialOriginalPositions[trial].size())
-            += trialOriginalPositions[trial];
+        desired.segment(0, trialOriginalPositions[trial].size()) +=
+            trialOriginalPositions[trial];
+        desired -= bFixedMass;
 
         // Old version, direct solve:
         // Eigen::VectorXs localResult
@@ -10734,8 +10738,7 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
         solver.setTolerance(1e-9);
         solver.setMaxIterations(200);
         solver.compute(AFixedMass);
-        Eigen::VectorXs localResult
-            = solver.solve(unifiedPositions - unifiedGravityOffset);
+        Eigen::VectorXs localResult = solver.solve(desired);
         Eigen::VectorXs recoveredTrajectory
             = AFixedMass * localResult + bFixedMass;
 
@@ -11030,7 +11033,9 @@ void DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
 // least `boundPush` distance away from their bounds. This makes subsequent
 // interior-point optimizations converge more quickly.
 void DynamicsFitter::multimassZeroLinearResidualsOnCOMTrajectory(
-    std::shared_ptr<DynamicsInitialization> init, s_t boundPush)
+    std::shared_ptr<DynamicsInitialization> init,
+    int maxTrialsToSolveMassOver,
+    s_t boundPush)
 {
   ResidualForceHelper helper(mSkeleton, init->grfBodyIndices);
 
@@ -11339,7 +11344,7 @@ void DynamicsFitter::multimassZeroLinearResidualsOnCOMTrajectory(
         std::cout << "Above regularization boundary. Falling back to a "
                      "linear solve (holding link masses constant)."
                   << std::endl;
-        zeroLinearResidualsOnCOMTrajectory(init);
+        zeroLinearResidualsOnCOMTrajectory(init, maxTrialsToSolveMassOver);
         return;
       }
       continue;
@@ -12026,8 +12031,10 @@ bool DynamicsFitter::timeSyncTrialGRF(
 bool DynamicsFitter::timeSyncAndInitializePipeline(
     std::shared_ptr<DynamicsInitialization> init,
     bool useReactionWheels,
+    bool shiftGRF,
     int maxShiftGRF,
     int iterationsPerShift,
+    int maxTrialsToSolveMassOver,
     s_t weightLinear,
     s_t weightAngular,
     s_t regularizeLinearResiduals,
@@ -12044,26 +12051,26 @@ bool DynamicsFitter::timeSyncAndInitializePipeline(
     originalPoseTrials.push_back(init->poseTrials[i]);
   }
   // First detect external force
-  zeroLinearResidualsOnCOMTrajectory(init);
+  zeroLinearResidualsOnCOMTrajectory(init, maxTrialsToSolveMassOver);
 
   // Now reset positions and re-run with multi-mass
   for (int i = 0; i < init->poseTrials.size(); i++)
   {
     init->poseTrials[i] = originalPoseTrials[i];
   }
-  multimassZeroLinearResidualsOnCOMTrajectory(init);
+  multimassZeroLinearResidualsOnCOMTrajectory(init, maxTrialsToSolveMassOver);
 
   // Attempt to time sync the GRFs relative to the coordinate data.
-  bool timeSyncSuccess = true;
-  for (int trial = 0; trial < init->poseTrials.size(); trial++)
+  if (shiftGRF)
   {
-    timeSyncSuccess = timeSyncTrialGRF(
-        init,
-        trial,
-        useReactionWheels,
-        maxShiftGRF,
-        iterationsPerShift);
-    if (!timeSyncSuccess) return false;
+    bool timeSyncSuccess = true;
+    for (int trial = 0; trial < init->poseTrials.size(); trial++)
+    {
+      timeSyncSuccess = timeSyncTrialGRF(
+          init, trial, useReactionWheels, maxShiftGRF, iterationsPerShift);
+      if (!timeSyncSuccess)
+        return false;
+    }
   }
 
   // Reset the pose trials now that we've found the GRF data, and start again
@@ -12072,7 +12079,7 @@ bool DynamicsFitter::timeSyncAndInitializePipeline(
     init->poseTrials[trial] = originalPoseTrials[trial];
   }
   // Re-find the link masses, with updated GRF offsets
-  multimassZeroLinearResidualsOnCOMTrajectory(init);
+  multimassZeroLinearResidualsOnCOMTrajectory(init, maxTrialsToSolveMassOver);
   init->regularizeGroupMassesTo = mSkeleton->getGroupMasses();
 
   // If we're using reaction wheels, we're accepting that you can't get this

dart/biomechanics/DynamicsFitter.hpp

@@ -1353,6 +1353,7 @@ class DynamicsFitter
   // the current kinematic fit.
   void zeroLinearResidualsOnCOMTrajectory(
       std::shared_ptr<DynamicsInitialization> init,
+      int maxTrialsToSolveMassOver = 4,
       bool detectExternalForce = true,
       int driftCorrectionBlurRadius = 250,
       int driftCorrectionBlurInterval = 250);
@@ -1365,7 +1366,9 @@ class DynamicsFitter
   // least `boundPush` distance away from their bounds. This makes subsequent
   // interior-point optimizations converge more quickly.
   void multimassZeroLinearResidualsOnCOMTrajectory(
-      std::shared_ptr<DynamicsInitialization> init, s_t boundPush = 0.01);
+      std::shared_ptr<DynamicsInitialization> init,
+      int maxTrialsToSolveMassOver = 4,
+      s_t boundPush = 0.01);
 
   // 1. Change the initial positions and velocities of the body to achieve a
   // least-squares closest COM trajectory to the current kinematic fit, taking
@@ -1412,8 +1415,10 @@ class DynamicsFitter
   bool timeSyncAndInitializePipeline(
       std::shared_ptr<DynamicsInitialization> init,
       bool useReactionWheels = false,
+      bool shiftGRF = false,
       int maxShiftGRF = 4,
       int iterationsPerShift = 20,
+      int maxTrialsToSolveMassOver = 4,
       s_t weightLinear = 1.0,
       s_t weightAngular = 0.5,
       s_t regularizeLinearResiduals = 0.1,

dart/biomechanics/OpenSimParser.cpp

@@ -104,6 +104,14 @@ std::string to_string(double d)
   return ss.str();
 }
 
+// https://www.geeksforgeeks.org/round-the-given-number-to-nearest-multiple-of-10/
+int roundToNearestMultiple(int n, int multiple)
+{
+    int a = (n / multiple) * multiple;
+    int b = a + multiple;
+    return (n - a > b - n) ? b : a;
+}
+
 //==============================================================================
 OpenSimFile::OpenSimFile(
     dynamics::SkeletonPtr skeleton, dynamics::MarkerMap markersMap)
@@ -2092,7 +2100,7 @@ OpenSimTRC OpenSimParser::loadTRC(
     int frames = result.timestamps.size();
     s_t elapsed = result.timestamps[result.timestamps.size() - 1]
                   - result.timestamps[0];
-    result.framesPerSecond = (int)round(frames / elapsed);
+    result.framesPerSecond = roundToNearestMultiple((int)(frames / elapsed), 10);
   }
 
   return result;
@@ -3027,7 +3035,7 @@ std::vector<ForcePlate> OpenSimParser::loadGRF(
   {
     int frames = timestamps.size();
     s_t elapsed = timestamps[timestamps.size() - 1] - timestamps[0];
-    int framesPerSecond = (int)round((double)frames / elapsed);
+    int framesPerSecond = roundToNearestMultiple((int)(frames / elapsed), 10);
     if (framesPerSecond < targetFramesPerSecond)
     {
       std::cout << "WARNING!!! OpenSimParser is trying to load "