Improvements to MocoUtilities::createExternalLoadsTableForGait()

CHANGELOG.md

@@ -23,6 +23,9 @@ v4.6
   components. The `ForceProducer` API was also rolled out to a variety of existing `Force` components, which
   means that API users can now now ask many `Force` components what forces they produce (see #3891 for a
   comprehensive overview).
+- Made improvements to `MocoUtilities::createExternalLoadsTableForGait()`: center of pressure values are now set to zero, rather 
+  than NaN, when vertical force is zero, and the vertical torque is returned in the torque columns (rather than the sum of the 
+  sphere torques) to be consistent with the center of pressure GRF representation.
 
 v4.5.1
 ======

OpenSim/Moco/MocoUtilities.cpp

@@ -350,26 +350,49 @@ TimeSeriesTable OpenSim::createExternalLoadsTableForGait(Model model,
                     forceValues[11]);
         }
 
-        // Compute centers of pressure for both feet. We need to use the force
-        // and torque information from the half space to compute the correct
-        // locations.
+        // The simulated ground reaction forces and moments are just the 
+        // opposite of the sum of forces and torques applied to the half-space.
+        SimTK::Vec3 forcesRight = -halfSpaceForcesRight;
+        SimTK::Vec3 forcesLeft = -halfSpaceForcesLeft;
+        SimTK::Vec3 momentsRight = -halfSpaceTorquesRight;
+        SimTK::Vec3 momentsLeft = -halfSpaceTorquesLeft;
+
+        // The ground reaction moments should be equal to the moment caused by 
+        // ground reaction force when applied at the center of pressure plus the 
+        // vertical torque (see http://www.kwon3d.com/theory/grf/cop.html). 
+        // Here, the vertical components of the forces and torques are in the 
+        // y-direction (i.e., the y-component of the center of pressure is zero).
         // TODO: Support contact plane normals in any direction.
+
+        // Compute centers of pressure.
         SimTK::Vec3 copRight(0);
-        copRight(0) = halfSpaceTorquesRight(2) / halfSpaceForcesRight(1);
-        copRight(2) = -halfSpaceTorquesRight(0) / halfSpaceForcesRight(1);
+        if (std::abs(forcesRight(1)) > SimTK::SignificantReal) {
+            copRight(0) = momentsRight(2) / forcesRight(1);
+            copRight(2) = -momentsRight(0) / forcesRight(1);
+        }
 
         SimTK::Vec3 copLeft(0);
-        copLeft(0) = halfSpaceTorquesLeft(2) / halfSpaceForcesLeft(1);
-        copLeft(2) = -halfSpaceTorquesLeft(0) / halfSpaceForcesLeft(1);
+        if (std::abs(forcesLeft(1)) > SimTK::SignificantReal) {
+            copLeft(0) = momentsLeft(2) / forcesLeft(1);
+            copLeft(2) = -momentsLeft(0) / forcesLeft(1);
+        }
+
+        // Calculate the vertical torques.
+        SimTK::Vec3 verticalTorqueRight(0);
+        verticalTorqueRight(1) = momentsRight(1) - copRight(2) * forcesRight(0)
+                + copRight(0) * forcesRight(2);
+        SimTK::Vec3 verticalTorqueLeft(0);
+        verticalTorqueLeft(1) = momentsLeft(1) - copLeft(2) * forcesLeft(0)
+                + copLeft(0) * forcesLeft(2);
 
         // Append row to table.
         SimTK::RowVector_<SimTK::Vec3> row(6);
-        row(0) = sphereForcesRight;
+        row(0) = forcesRight;
         row(1) = copRight;
-        row(2) = sphereForcesLeft;
+        row(2) = forcesLeft;
         row(3) = copLeft;
-        row(4) = sphereTorquesRight;
-        row(5) = sphereTorquesLeft;
+        row(4) = verticalTorqueRight;
+        row(5) = verticalTorqueLeft;
         externalForcesTable.appendRow(state.getTime(), row);
     }
     // Create table.

OpenSim/Moco/MocoUtilities.h

@@ -250,7 +250,9 @@ class FileDeletionThrower {
 /// distinguish between right ("<>_r") and left ("<>_l") forces, centers of
 /// pressure, and torques. Centers of pressure are computed assuming the
 /// that the contact plane's normal is in the y-direction, which is the OpenSim
-/// convention.
+/// convention. Torques are computed based on the center of pressure
+/// representation of the ground reaction forces: a "vertical" torque in the 
+/// y-direction and zero torque in the other directions.
 ///
 /// The forces and torques are computed from the first six outputs of
 /// getRecordValues(), while the centers of pressure are computed from the second

OpenSim/Moco/Test/CMakeLists.txt

@@ -51,7 +51,9 @@ MocoAddTest(NAME testMocoImplicit)
 
 MocoAddTest(NAME testMocoConstraints)
 
-MocoAddTest(NAME testMocoContact)
+MocoAddTest(NAME testMocoContact RESOURCES
+        subject_20dof18musc_running.osim
+        running_solution_full_stride.sto)
 
 MocoAddTest(NAME testMocoControllers)