Fixing an IPOPT bug, and adding more APIs

dart/biomechanics/DynamicsFitter.cpp

@@ -1,6 +1,7 @@
 #include "dart/biomechanics/DynamicsFitter.hpp"
 
 #include <algorithm>
+#include <climits>
 #include <future>
 #include <iostream>
 #include <limits>
@@ -8926,9 +8927,26 @@ bool DynamicsFitProblem::get_nlp_info(
   // Set the number of entries in the Hessian
   nnz_h_lag = n * n;
 
+  if ((int32_t)nnz_h_lag < 0 || nnz_h_lag >= std::numeric_limits<int32_t>::max()
+      || n > 46340)
+  {
+    // We've got so many dimensions that we can't store the size of the full
+    // hessian in 32-bits.
+    std::cout << "WARNING: nnz_h_lag is too large to store in signed 32-bits. "
+                 "Setting "
+                 "to 1."
+              << std::endl;
+    nnz_h_lag = 1;
+  }
+
   // use the C style indexing (0-based)
   index_style = Ipopt::TNLP::C_STYLE;
 
+  std::cout << "DynamicsFitProbelem: Getting NLP info" << std::endl;
+  std::cout << "   n=" << n << " m=" << m << std::endl;
+  std::cout << "   nnz_jac_g=" << nnz_jac_g << " nnz_h_lag=" << nnz_h_lag
+            << std::endl;
+
   return true;
 }
 

dart/biomechanics/SubjectOnDisk.cpp

@@ -2554,8 +2554,6 @@ void SubjectOnDiskTrialPass::computeValuesFromForcePlates(
           }
         }
 
-        // TODO
-        /*
 #ifndef NDEBUG
         // Check that inverse dynamics given these inputs produces the expected
         // joint torques
@@ -2583,7 +2581,6 @@ void SubjectOnDiskTrialPass::computeValuesFromForcePlates(
           assert(false);
         }
 #endif
-        */
       }
     }
     linearResiduals.push_back(linearResidual);

python/_nimblephysics/biomechanics/DynamicsFitter.cpp

@@ -106,7 +106,40 @@ void DynamicsFitter(py::module& m)
           ::py::arg("forcesConcat"),
           ::py::arg("wrt"),
           ::py::arg("torquesMultiple"),
-          ::py::arg("useL1") = false);
+          ::py::arg("useL1") = false)
+      .def(
+          "calculateComToCenterAngularResiduals",
+          &dart::biomechanics::ResidualForceHelper::
+              calculateComToCenterAngularResiduals,
+          ::py::arg("q"),
+          ::py::arg("dq"),
+          ::py::arg("ddq"),
+          ::py::arg("forcesConcat"),
+          "This computes the location that we would need to move the COM to in "
+          "order to center the angular residuals. Moving the COM to the "
+          "computed location doesn't remove angular residuals, but ensures "
+          "that any remaining residuals are parallel to the net external force "
+          "on the body.")
+      .def(
+          "calculateCOMAngularResidual",
+          &dart::biomechanics::ResidualForceHelper::calculateCOMAngularResidual,
+          ::py::arg("q"),
+          ::py::arg("dq"),
+          ::py::arg("ddq"),
+          ::py::arg("forcesConcat"),
+          "This computes the residual at the root, then transforms that to the "
+          "COM and expresses the torque as a spatial vector (even if the root "
+          "joint uses euler coordinates for rotation).")
+      .def(
+          "calculateResidualFreeRootAcceleration",
+          &dart::biomechanics::ResidualForceHelper::
+              calculateResidualFreeRootAcceleration,
+          ::py::arg("q"),
+          ::py::arg("dq"),
+          ::py::arg("ddq"),
+          ::py::arg("forcesConcat"),
+          "This computes the acceleration we would need at the root in order "
+          "to remove all residual forces.");
 
   ::py::class_<
       dart::biomechanics::DynamicsInitialization,

stubs/_nimblephysics-stubs/biomechanics/__init__.pyi

@@ -2427,8 +2427,20 @@ class ProcessingPassType():
     pass
 class ResidualForceHelper():
     def __init__(self, skeleton: nimblephysics_libs._nimblephysics.dynamics.Skeleton, forceBodies: typing.List[int]) -> None: ...
+    def calculateCOMAngularResidual(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]: 
+        """
+        This computes the residual at the root, then transforms that to the COM and expresses the torque as a spatial vector (even if the root joint uses euler coordinates for rotation).
+        """
+    def calculateComToCenterAngularResiduals(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]: 
+        """
+        This computes the location that we would need to move the COM to in order to center the angular residuals. Moving the COM to the computed location doesn't remove angular residuals, but ensures that any remaining residuals are parallel to the net external force on the body.
+        """
     def calculateInverseDynamics(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: ...
     def calculateResidual(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[6, 1]]: ...
+    def calculateResidualFreeRootAcceleration(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[6, 1]]: 
+        """
+        This computes the acceleration we would need at the root in order to remove all residual forces.
+        """
     def calculateResidualJacobianWrt(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]], wrt: nimblephysics_libs._nimblephysics.neural.WithRespectTo) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: ...
     def calculateResidualNorm(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]], torquesMultiple: float, useL1: bool = False) -> float: ...
     def calculateResidualNormGradientWrt(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]], wrt: nimblephysics_libs._nimblephysics.neural.WithRespectTo, torquesMultiple: float, useL1: bool = False) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: ...

stubs/_nimblephysics-stubs/utils/__init__.pyi

@@ -10,6 +10,8 @@ _Shape = typing.Tuple[int, ...]
 __all__ = [
     "AccelerationMinimizer",
     "AccelerationSmoother",
+    "AccelerationTrackAndMinimize",
+    "AccelerationTrackingResult",
     "DartLoader",
     "MJCFExporter",
     "SdfParser",
@@ -32,6 +34,31 @@ class AccelerationSmoother():
     def setIterations(self, iterations: int) -> None: ...
     def smooth(self, series: numpy.ndarray[numpy.float64, _Shape[m, n]]) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: ...
     pass
+class AccelerationTrackAndMinimize():
+    def __init__(self, numTimesteps: int, trackAccelerationAtTimesteps: typing.List[bool], zeroUnobservedAccWeight: float = 1.0, trackObservedAccWeight: float = 1.0, regularizationWeight: float = 0.01, dt: float = 1.0, numIterations: int = 10000) -> None: ...
+    def minimize(self, series: numpy.ndarray[numpy.float64, _Shape[m, 1]], trackAcc: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> AccelerationTrackingResult: ...
+    def setConvergenceTolerance(self, tolerance: float) -> None: ...
+    def setDebugIterationBackoff(self, iterations: bool) -> None: ...
+    def setNumIterationsBackoff(self, series: int) -> None: ...
+    pass
+class AccelerationTrackingResult():
+    @property
+    def accelerationOffset(self) -> float:
+        """
+        :type: float
+        """
+    @accelerationOffset.setter
+    def accelerationOffset(self, arg0: float) -> None:
+        pass
+    @property
+    def series(self) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]:
+        """
+        :type: numpy.ndarray[numpy.float64, _Shape[m, 1]]
+        """
+    @series.setter
+    def series(self, arg0: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> None:
+        pass
+    pass
 class DartLoader():
     def __init__(self) -> None: ...
     def addPackageDirectory(self, packageName: str, packageDirectory: str) -> None: ...