Updating ExoSolverPinnedContact to make sure zero torques are read on…

… identical models

dart/exo/ExoSolverPinnedContact.cpp

@@ -184,6 +184,25 @@ Eigen::VectorXs ExoSolverPinnedContact::estimateHumanTorques(
   return tau;
 }
 
+//==============================================================================
+/// This is part of the main exoskeleton solver. It takes in the current
+/// joint velocities and accelerations, and returns the estimated total
+/// joint torques for the human + exoskeleton system.
+Eigen::VectorXs ExoSolverPinnedContact::estimateTotalTorques(
+    Eigen::VectorXs dq, Eigen::VectorXs ddq, Eigen::VectorXs contactForces)
+{
+  mRealSkel->setVelocities(dq);
+
+  Eigen::MatrixXs M = mRealSkel->getMassMatrix();
+  Eigen::VectorXs C = mRealSkel->getCoriolisAndGravityForces()
+                      - mRealSkel->getExternalForces();
+  Eigen::VectorXs contactJointTorques
+      = getContactJacobian().transpose() * contactForces;
+  Eigen::VectorXs tau = M * ddq + C - contactJointTorques;
+
+  return tau;
+}
+
 //==============================================================================
 /// This is part of the main exoskeleton solver. It takes in the current
 /// estimated human pilot joint torques, and computes the accelerations we
@@ -311,9 +330,13 @@ ExoSolverPinnedContact::getPinnedRealDynamicsLinearMap(Eigen::VectorXs dq)
 /// This is part of the main exoskeleton solver. It takes in how the digital
 /// twin of the exo pilot is accelerating, and attempts to solve for the
 /// torques that the exo needs to apply to get as close to that as possible.
+/// It resolves ambiguities by minimizing the exo torques.
 std::pair<Eigen::VectorXs, Eigen::VectorXs>
 ExoSolverPinnedContact::getPinnedTotalTorques(
-    Eigen::VectorXs dq, Eigen::VectorXs ddqDesired)
+    Eigen::VectorXs dq,
+    Eigen::VectorXs ddqDesired,
+    Eigen::VectorXs centeringTau,
+    Eigen::VectorXs centeringForces)
 {
   mRealSkel->setVelocities(dq);
 
@@ -334,7 +357,14 @@ ExoSolverPinnedContact::getPinnedTotalTorques(
   A.block(0, numDofs, contactJointTorques.cols(), contactJointTorques.rows())
       = contactJointTorques.transpose();
 
-  Eigen::VectorXs solution = A.completeOrthogonalDecomposition().solve(b);
+  Eigen::VectorXs centering
+      = Eigen::VectorXs::Zero(numDofs + contactJointTorques.rows());
+  centering.segment(0, numDofs) = centeringTau;
+  centering.segment(numDofs, contactJointTorques.rows()) = centeringForces;
+
+  Eigen::VectorXs solution
+      = A.completeOrthogonalDecomposition().solve(b - A * centering)
+        + centering;
 
   Eigen::VectorXs tauTotal = solution.segment(0, numDofs);
   Eigen::VectorXs f = solution.segment(numDofs, contactJointTorques.rows());
@@ -396,6 +426,40 @@ ExoSolverPinnedContact::projectTorquesToExoControlSpaceLinearMap()
   return J.completeOrthogonalDecomposition().pseudoInverse();
 }
 
+//==============================================================================
+/// Often our estimates for `dq` and `ddq` violate the pin constraints. That
+/// leads to exo torques that do not tend to zero as the virtual human exactly
+/// matches the real human+exo system. To solve this problem, we can solve a
+/// set of least-squares equations to find the best set of ddq values to
+/// satisfy the constraint.
+Eigen::VectorXs ExoSolverPinnedContact::
+    getClosestRealAccelerationConsistentWithPinsAndContactForces(
+        Eigen::VectorXs dq, Eigen::VectorXs ddq, Eigen::VectorXs contactForces)
+{
+  mRealSkel->setVelocities(dq);
+
+  Eigen::MatrixXs J = getContactJacobian();
+  Eigen::MatrixXs M = mRealSkel->getMassMatrix();
+  Eigen::VectorXs C = mRealSkel->getCoriolisAndGravityForces()
+                      - mRealSkel->getExternalForces();
+  Eigen::VectorXs contactTau = J.transpose() * contactForces;
+
+  const int numDofs = dq.size();
+
+  Eigen::MatrixXs A = Eigen::MatrixXs::Zero(6 + J.rows(), numDofs);
+  A.block(0, 0, 6, numDofs) = M.block(0, 0, 6, numDofs);
+  A.block(6, 0, J.rows(), numDofs) = J;
+
+  Eigen::VectorXs b = Eigen::VectorXs::Zero(6 + J.rows());
+  b.segment<6>(0) = contactTau.segment<6>(0) - C.segment<6>(0);
+
+  // Solve, centered on ddq
+  Eigen::VectorXs solution
+      = A.completeOrthogonalDecomposition().solve(b - A * ddq) + ddq;
+
+  return solution;
+}
+
 //==============================================================================
 /// This runs the entire exoskeleton solver pipeline, spitting out the
 /// torques to apply to the exoskeleton actuators.
@@ -405,20 +469,25 @@ Eigen::VectorXs ExoSolverPinnedContact::solveFromAccelerations(
     Eigen::VectorXs lastExoTorques,
     Eigen::VectorXs contactForces)
 {
+  Eigen::VectorXs systemTau = estimateTotalTorques(dq, ddq, contactForces);
   Eigen::VectorXs humanTau
       = estimateHumanTorques(dq, ddq, contactForces, lastExoTorques);
-  return solveFromBiologicalTorques(dq, humanTau);
+  return solveFromBiologicalTorques(dq, humanTau, systemTau, contactForces);
 }
 
 //==============================================================================
 /// This is a subset of the steps in solveFromAccelerations, which can take
 /// the biological joint torques directly, and solve for the exo torques.
 Eigen::VectorXs ExoSolverPinnedContact::solveFromBiologicalTorques(
-    Eigen::VectorXs dq, Eigen::VectorXs humanTau)
+    Eigen::VectorXs dq,
+    Eigen::VectorXs humanTau,
+    Eigen::VectorXs centeringTau,
+    Eigen::VectorXs centeringForces)
 {
   Eigen::VectorXs virtualDdq = getPinnedVirtualDynamics(dq, humanTau).first;
   Eigen::VectorXs totalRealTorques
-      = getPinnedTotalTorques(dq, virtualDdq).first;
+      = getPinnedTotalTorques(dq, virtualDdq, centeringTau, centeringForces)
+            .first;
   Eigen::VectorXs netTorques = totalRealTorques - humanTau;
   Eigen::VectorXs exoTorques = projectTorquesToExoControlSpace(netTorques);
   return exoTorques;
@@ -459,7 +528,8 @@ std::tuple<Eigen::VectorXs, Eigen::VectorXs, Eigen::VectorXs>
 ExoSolverPinnedContact::getPinnedForwardDynamicsForExoAndHuman(
     Eigen::VectorXs dq, Eigen::VectorXs humanTau)
 {
-  Eigen::VectorXs exoTau = solveFromBiologicalTorques(dq, humanTau);
+  Eigen::VectorXs exoTau = solveFromBiologicalTorques(
+      dq, humanTau, humanTau, Eigen::VectorXs::Zero(3 * mPins.size()));
   Eigen::VectorXs exoTauOnJoints = getExoToJointTorquesJacobian() * exoTau;
   Eigen::VectorXs totalTau = humanTau + exoTauOnJoints;
   std::pair<Eigen::VectorXs, Eigen::VectorXs> ddqAndForces
@@ -520,7 +590,8 @@ ExoSolverPinnedContact::getHumanAndExoTorques(
             .solve(ddq - humanTorquesLinearForwardDynamics.second);
 
   // Reconstruct the exo torque from the human torque
-  Eigen::VectorXs exoTau = solveFromBiologicalTorques(dq, humanTau);
+  Eigen::VectorXs exoTau = solveFromBiologicalTorques(
+      dq, humanTau, humanTau, Eigen::VectorXs::Zero(3 * mPins.size()));
 
   return std::make_pair(humanTau, exoTau);
 }

dart/exo/ExoSolverPinnedContact.hpp

@@ -65,6 +65,12 @@ class ExoSolverPinnedContact
       Eigen::VectorXs contactForces,
       Eigen::VectorXs lastExoTorques);
 
+  /// This is part of the main exoskeleton solver. It takes in the current
+  /// joint velocities and accelerations, and returns the estimated total
+  /// joint torques for the human + exoskeleton system.
+  Eigen::VectorXs estimateTotalTorques(
+      Eigen::VectorXs dq, Eigen::VectorXs ddq, Eigen::VectorXs contactForces);
+
   /// This is part of the main exoskeleton solver. It takes in the current
   /// estimated human pilot joint torques, and computes the accelerations we
   /// would see on the virtual skeleton if we applied those same torques, with
@@ -93,8 +99,12 @@ class ExoSolverPinnedContact
   /// This is part of the main exoskeleton solver. It takes in how the digital
   /// twin of the exo pilot is accelerating, and attempts to solve for the
   /// torques that the exo needs to apply to get as close to that as possible.
+  /// It resolves ambiguities by minimizing the change in total system torques.
   std::pair<Eigen::VectorXs, Eigen::VectorXs> getPinnedTotalTorques(
-      Eigen::VectorXs dq, Eigen::VectorXs ddqDesired);
+      Eigen::VectorXs dq,
+      Eigen::VectorXs ddqDesired,
+      Eigen::VectorXs centeringTau,
+      Eigen::VectorXs centeringForces);
 
   /// This does the same thing as getPinnedTotalTorques, but returns the Ax +
   /// b values A and b such that Ax + b = tau, accounting for the pin
@@ -111,6 +121,14 @@ class ExoSolverPinnedContact
   /// the matrix to multiply by the torques to get the exo torques.
   Eigen::MatrixXs projectTorquesToExoControlSpaceLinearMap();
 
+  /// Often our estimates for `dq` and `ddq` violate the pin constraints. That
+  /// leads to exo torques that do not tend to zero as the virtual human exactly
+  /// matches the real human+exo system. To solve this problem, we can solve a
+  /// set of least-squares equations to find the best set of ddq values to
+  /// satisfy the constraint.
+  Eigen::VectorXs getClosestRealAccelerationConsistentWithPinsAndContactForces(
+      Eigen::VectorXs dq, Eigen::VectorXs ddq, Eigen::VectorXs contactForces);
+
   /// This runs the entire exoskeleton solver pipeline, spitting out the
   /// torques to apply to the exoskeleton actuators.
   Eigen::VectorXs solveFromAccelerations(
@@ -122,7 +140,10 @@ class ExoSolverPinnedContact
   /// This is a subset of the steps in solveFromAccelerations, which can take
   /// the biological joint torques directly, and solve for the exo torques.
   Eigen::VectorXs solveFromBiologicalTorques(
-      Eigen::VectorXs dq, Eigen::VectorXs tau);
+      Eigen::VectorXs dq,
+      Eigen::VectorXs humanTau,
+      Eigen::VectorXs centeringTau,
+      Eigen::VectorXs centeringForces);
 
   /// This is the same as solveFromBiologicalTorques, but returns the Ax + b
   /// values A and b such that Ax + b = exo_tau, accounting for the pin

python/_nimblephysics/dynamics/Skeleton.cpp

@@ -1096,6 +1096,7 @@ void Skeleton(
               bool scaleBodies,
               double convergenceThreshold,
               int maxStepCount,
+              int numIndependentStarts,
               double leastSquaresDamping,
               bool lineSearch,
               bool logOutput) -> double {
@@ -1108,6 +1109,7 @@ void Skeleton(
                     .setConvergenceThreshold(convergenceThreshold)
                     .setMaxStepCount(maxStepCount)
                     .setLeastSquaresDamping(leastSquaresDamping)
+                    .setMaxRestarts(numIndependentStarts)
                     .setLineSearch(lineSearch)
                     .setLogOutput(logOutput));
           },
@@ -1117,6 +1119,7 @@ void Skeleton(
           ::py::arg("scaleBodies") = false,
           ::py::arg("convergenceThreshold") = 1e-7,
           ::py::arg("maxStepCount") = 100,
+          ::py::arg("numIndependentStarts") = 1,
           ::py::arg("leastSquaresDamping") = 0.01,
           ::py::arg("lineSearch") = true,
           ::py::arg("logOutput") = false)

python/_nimblephysics/exo/ExoSolverPinnedContact.cpp

@@ -112,6 +112,15 @@ void ExoSolverPinnedContact(py::module& m)
           "current joint velocities and accelerations, and the last "
           "exoskeleton torques, and returns the estimated human pilot joint "
           "torques.")
+      .def(
+          "estimateTotalTorques",
+          &dart::exo::ExoSolverPinnedContact::estimateTotalTorques,
+          ::py::arg("dq"),
+          ::py::arg("ddq"),
+          ::py::arg("contactForces"),
+          "This is part of the main exoskeleton solver. It takes in the "
+          "current joint velocities and accelerations, and returns the "
+          "estimated human + exo system joint torques.")
       .def(
           "getPinnedVirtualDynamics",
           &dart::exo::ExoSolverPinnedContact::getPinnedVirtualDynamics,
@@ -148,6 +157,8 @@ void ExoSolverPinnedContact(py::module& m)
           &dart::exo::ExoSolverPinnedContact::getPinnedTotalTorques,
           ::py::arg("dq"),
           ::py::arg("ddqDesired"),
+          ::py::arg("centeringTau"),
+          ::py::arg("centeringForces"),
           "This is part of the main exoskeleton solver. It takes in how the "
           "digital twin of the exo pilot is accelerating, and attempts to "
           "solve for the torques that the exo needs to apply to get as close "
@@ -166,6 +177,19 @@ void ExoSolverPinnedContact(py::module& m)
           "This is part of the main exoskeleton solver. It takes in the "
           "desired torques for the exoskeleton, and returns the torques on the "
           "actuated DOFs that can be used to drive the exoskeleton.")
+      .def(
+          "getClosestRealAccelerationConsistentWithPinsAndContactForces",
+          &dart::exo::ExoSolverPinnedContact::
+              getClosestRealAccelerationConsistentWithPinsAndContactForces,
+          ::py::arg("dq"),
+          ::py::arg("ddq"),
+          ::py::arg("contactForces"),
+          "Often our estimates for `dq` and `ddq` violate the pin "
+          "constraints. That leads to exo torques that do not tend to zero "
+          "as the virtual human exactly matches the real human+exo system. "
+          "To solve this problem, we can solve a set of least-squares "
+          "equations to find the best set of ddq values to satisfy the "
+          "constraint.")
       .def(
           "projectTorquesToExoControlSpaceLinearMap",
           &dart::exo::ExoSolverPinnedContact::
@@ -187,6 +211,8 @@ void ExoSolverPinnedContact(py::module& m)
           &dart::exo::ExoSolverPinnedContact::solveFromBiologicalTorques,
           ::py::arg("dq"),
           ::py::arg("tau"),
+          ::py::arg("centeringTau"),
+          ::py::arg("centeringForce"),
           "This is a subset of the steps in solveFromAccelerations, which can "
           "take the biological joint torques directly, and solve for the exo "
           "torques.")

stubs/_nimblephysics-stubs/dynamics/__init__.pyi

@@ -4723,7 +4723,7 @@ class Skeleton(MetaSkeleton):
     def enableAdjacentBodyCheck(self) -> None: ...
     def enableSelfCollisionCheck(self) -> None: ...
     def fitJointsToWorldPositions(self, positionJoints: typing.List[Joint], targetPositions: numpy.ndarray[numpy.float64, _Shape[m, 1]], scaleBodies: bool = False, convergenceThreshold: float = 1e-07, maxStepCount: int = 100, leastSquaresDamping: float = 0.01, lineSearch: bool = True, logOutput: bool = False) -> float: ...
-    def fitMarkersToWorldPositions(self, markers: typing.List[typing.Tuple[BodyNode, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], targetPositions: numpy.ndarray[numpy.float64, _Shape[m, 1]], markerWeights: numpy.ndarray[numpy.float64, _Shape[m, 1]], scaleBodies: bool = False, convergenceThreshold: float = 1e-07, maxStepCount: int = 100, leastSquaresDamping: float = 0.01, lineSearch: bool = True, logOutput: bool = False) -> float: ...
+    def fitMarkersToWorldPositions(self, markers: typing.List[typing.Tuple[BodyNode, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], targetPositions: numpy.ndarray[numpy.float64, _Shape[m, 1]], markerWeights: numpy.ndarray[numpy.float64, _Shape[m, 1]], scaleBodies: bool = False, convergenceThreshold: float = 1e-07, maxStepCount: int = 100, numIndependentStarts: int = 1, leastSquaresDamping: float = 0.01, lineSearch: bool = True, logOutput: bool = False) -> float: ...
     def getAccMapReadings(self, accelerometers: typing.Dict[str, typing.Tuple[BodyNode, nimblephysics_libs._nimblephysics.math.Isometry3]]) -> typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]: 
         """
         These are a set of bodies, and offsets in local body space where gyros

stubs/_nimblephysics-stubs/exo/__init__.pyi

@@ -25,10 +25,18 @@ class ExoSolverPinnedContact():
         """
         This is part of the main exoskeleton solver. It takes in the current joint velocities and accelerations, and the last exoskeleton torques, and returns the estimated human pilot joint torques.
         """
+    def estimateTotalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], contactForces: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: 
+        """
+        This is part of the main exoskeleton solver. It takes in the current joint velocities and accelerations, and returns the estimated human + exo system joint torques.
+        """
     def finiteDifferenceContactJacobian(self) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: 
         """
         This is only used for testing: Get the Jacobian relating world space velocity of the contact points to joint velocities, by finite differencing.
         """
+    def getClosestRealAccelerationConsistentWithPinsAndContactForces(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], contactForces: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: 
+        """
+        Often our estimates for `dq` and `ddq` violate the pin constraints. That leads to exo torques that do not tend to zero as the virtual human exactly matches the real human+exo system. To solve this problem, we can solve a set of least-squares equations to find the best set of ddq values to satisfy the constraint.
+        """
     def getContactJacobian(self) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: 
         """
         Get the Jacobian relating world space velocity of the contact points to joint velocities.
@@ -58,7 +66,7 @@ class ExoSolverPinnedContact():
         """
         This does the same thing as getPinndRealDynamics, but returns the Ax + b values A and b such that Ax + b = ddq, accounting for the pin constraints.
         """
-    def getPinnedTotalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddqDesired: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> typing.Tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], numpy.ndarray[numpy.float64, _Shape[m, 1]]]: 
+    def getPinnedTotalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddqDesired: numpy.ndarray[numpy.float64, _Shape[m, 1]], centeringTau: numpy.ndarray[numpy.float64, _Shape[m, 1]], centeringForces: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> typing.Tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], numpy.ndarray[numpy.float64, _Shape[m, 1]]]: 
         """
         This is part of the main exoskeleton solver. It takes in how the digital twin of the exo pilot is accelerating, and attempts to solve for the torques that the exo needs to apply to get as close to that as possible.
         """
@@ -95,7 +103,7 @@ class ExoSolverPinnedContact():
         """
         This runs the entire exoskeleton solver pipeline, spitting out the torques to apply to the exoskeleton actuators.
         """
-    def solveFromBiologicalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], tau: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: 
+    def solveFromBiologicalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], tau: numpy.ndarray[numpy.float64, _Shape[m, 1]], centeringTau: numpy.ndarray[numpy.float64, _Shape[m, 1]], centeringForce: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: 
         """
         This is a subset of the steps in solveFromAccelerations, which can take the biological joint torques directly, and solve for the exo torques.
         """