A bunch of small bounds checks and debug statements to aid when debug…

…ging AddBiomechanics bugs

dart/biomechanics/IKInitializer.cpp

@@ -720,6 +720,8 @@ s_t IKInitializer::prescaleBasedOnAnatomicalMarkers(bool logOutput)
       defaultScale = mModelHeightM / defaultHeight;
     }
   }
+  std::cout << "[IKInitializer] Default body node scale is " << defaultScale
+            << std::endl;
   mSkel->setBodyScales(originalBodyScales);
 
   for (auto& stackedBody : mStackedBodies)
@@ -827,6 +829,8 @@ s_t IKInitializer::prescaleBasedOnAnatomicalMarkers(bool logOutput)
         pairDistancesWithWeights,
         defaultScale,
         logOutput);
+    std::cout << "Prescaling body \"" << stackedBody->name << "\" by "
+              << scale.transpose() << std::endl;
 
     // Now we scale the body
     for (auto& body : stackedBody->bodies)
@@ -2186,6 +2190,9 @@ void IKInitializer::estimateGroupScalesClosedForm(bool log)
       defaultScale = mModelHeightM / defaultHeight;
     }
   }
+  std::cout
+      << "[IKInitializer::estimateGroupScalesClosedForm()] Default scale: "
+      << defaultScale << std::endl;
 
   // 1. Find a scale for all the bodies that we can
   std::map<std::string, Eigen::Vector3s> bodyScales;
@@ -2334,10 +2341,35 @@ void IKInitializer::estimateGroupScalesClosedForm(bool log)
       throw std::runtime_error(
           "Scale has NaN inside IKInitializer::estimateGroupScalesClosedForm!");
     }
+    if (scale.norm() < 1e-6)
+    {
+      std::cout << "Scale is zero inside "
+                   "IKInitializer::estimateGroupScalesClosedForm!"
+                << std::endl;
+      throw std::runtime_error(
+          "Scale is zero inside IKInitializer::estimateGroupScalesClosedForm!");
+    }
+    if (scale.norm() > 10.0)
+    {
+      std::cout << "Scale is huge (" << scale.transpose()
+                << ") inside "
+                   "inside IKInitializer::estimateGroupScalesClosedForm for "
+                   "body nodes: "
+                << std::endl;
+      for (dynamics::BodyNode* body : bodyNode->bodies)
+      {
+        std::cout << "  " << body->getName() << std::endl;
+      }
+      throw std::runtime_error(
+          "Scale is huge inside IKInitializer::estimateGroupScalesClosedForm!");
+    }
 
     // 1.5. Apply that scale to all the bodies in this stacked body
     for (dynamics::BodyNode* body : bodyNode->bodies)
     {
+      std::cout
+          << "[IKInitializer::estimateGroupScalesClosedForm()] Setting scale "
+          << scale.transpose() << " for body " << body->getName() << std::endl;
       body->setScale(scale);
     }
   }

dart/biomechanics/SubjectOnDisk.cpp

@@ -782,6 +782,16 @@ std::shared_ptr<dynamics::Skeleton> SubjectOnDisk::readSkel(
     geometryFolder = common::Uri::createFromRelativeUri(mPath, "./Geometry/")
                          .getFilesystemPath();
   }
+  if (passNumberToLoad == -1)
+  {
+    passNumberToLoad = mHeader->mPasses.size() - 1;
+  }
+  if (passNumberToLoad >= mHeader->mPasses.size() || passNumberToLoad < 0)
+  {
+    std::cout << "SubjectOnDisk::readSkel() called with invalid pass number: "
+              << passNumberToLoad << std::endl;
+    return nullptr;
+  }
 
   tinyxml2::XMLDocument osimFile;
   osimFile.Parse(mHeader->mPasses[passNumberToLoad]->mOpenSimFileText.c_str());

dart/dynamics/BodyNode.cpp

@@ -488,14 +488,21 @@ void BodyNode::setScale(Eigen::Vector3s newScale, bool silentlyClamp)
 {
   for (int i = 0; i < 3; i++)
   {
+    if (!isfinite(newScale(i)))
+    {
+      std::cout << "BodyNode " << getName() << " refusing to setScale("
+                << newScale(i) << ", axis=" << i << ") because " << newScale(i)
+                << " is not finite." << std::endl;
+      return;
+    }
     if (newScale(i) < mScaleLowerBound(i))
     {
       // Don't warn if it's close, or if we're explicitly silent
       if (newScale(i) < mScaleLowerBound(i) - 0.001 && !silentlyClamp)
       {
-        std::cout << "BodyNode refusing to setScale(" << newScale(i)
-                  << ", axis=" << i << ") because " << newScale(i)
-                  << " is less than the scale lower bound ("
+        std::cout << "BodyNode " << getName() << " refusing to setScale("
+                  << newScale(i) << ", axis=" << i << ") because "
+                  << newScale(i) << " is less than the scale lower bound ("
                   << mScaleLowerBound(i) << "). Clamping to lower bound."
                   << std::endl;
       }
@@ -506,8 +513,8 @@ void BodyNode::setScale(Eigen::Vector3s newScale, bool silentlyClamp)
       // Don't warn if it's close, or if we're explicitly silent
       if (newScale(i) > mScaleUpperBound(i) + 0.001 && !silentlyClamp)
       {
-        std::cout << "BodyNode refusing to setScale(" << newScale
-                  << ", axis=" << i << ") because " << newScale
+        std::cout << "BodyNode " << getName() << " refusing to setScale("
+                  << newScale << ", axis=" << i << ") because " << newScale
                   << " is greater than the scale upper bound ("
                   << mScaleUpperBound << "). Clamping to upper bound."
                   << std::endl;
@@ -557,7 +564,8 @@ void BodyNode::setScale(Eigen::Vector3s newScale, bool silentlyClamp)
     {
       if (ratio(0) != ratio(1) || ratio(1) != ratio(2))
       {
-        std::cout << "WARNING: BodyNode attempting to setScale(" << newScale
+        std::cout << "WARNING: BodyNode " << getName()
+                  << " attempting to setScale(" << newScale
                   << ") but we're scaling an attached sphere shape, which "
                      "can't skew. Scaling by X-axis, arbitrarily."
                   << std::endl;
@@ -569,7 +577,8 @@ void BodyNode::setScale(Eigen::Vector3s newScale, bool silentlyClamp)
     {
       if (ratio(0) != ratio(2))
       {
-        std::cout << "WARNING: BodyNode attempting to setScale(" << newScale
+        std::cout << "WARNING: BodyNode " << getName()
+                  << " attempting to setScale(" << newScale
                   << ") but we're scaling an attached capsule shape, which "
                      "can't skew by a different X and Z. Scaling radius by "
                      "X-axis, height by Y-axis."

dart/dynamics/Skeleton.cpp

@@ -2528,6 +2528,8 @@ s_t Skeleton::getHeight(Eigen::VectorXs pose, Eigen::Vector3s up)
   Eigen::Vector3s minVertex = Eigen::Vector3s::Zero();
   dynamics::BodyNode* minBodyNode = nullptr;
 
+  int numMeshes = 0;
+  int numVertices = 0;
   for (int i = 0; i < getNumBodyNodes(); i++)
   {
     dynamics::BodyNode* node = getBodyNode(i);
@@ -2539,8 +2541,20 @@ s_t Skeleton::getHeight(Eigen::VectorXs pose, Eigen::Vector3s up)
       {
         dynamics::MeshShape* mesh
             = static_cast<dynamics::MeshShape*>(shape.get());
+        Eigen::Vector3s scale = mesh->getScale();
+        if (!isfinite(scale[0]) || !isfinite(scale[1]) || !isfinite(scale[2]))
+        {
+          std::cout << "WARNING: getHeight() found a mesh with a non-finite "
+                       "scale, attached to body node "
+                    << node->getName()
+                    << ". This means that the skeleton is in an invalid state. Exiting..."
+                    << std::endl;
+      throw std::runtime_error("getHeight() computed a height of 0");
+        }
+        numMeshes++;
         for (Eigen::Vector3s rawVertex : mesh->getVertices())
         {
+          numVertices++;
           Eigen::Vector3s vertex = mesh->getScale().cwiseProduct(rawVertex);
           Eigen::Vector3s worldVertex = shapeNode->getWorldTransform() * vertex;
           s_t upDist = up.dot(worldVertex);
@@ -2576,7 +2590,24 @@ s_t Skeleton::getHeight(Eigen::VectorXs pose, Eigen::Vector3s up)
 
   setPositions(originalPose);
 
+  if (numMeshes == 0 || numVertices == 0)
+  {
+    std::cout << "WARNING: getHeight() found no mesh shapes in the Skeleton, "
+                 "or only empty mesh shapes with no vertices. numMeshes="
+              << numMeshes << ", numVertices=" << numVertices
+              << ". "
+                 "Exiting..."
+              << std::endl;
+    throw std::runtime_error("getHeight() computed a height of 0");
+  }
+
   s_t height = 0;
+  if (!isfinite(maxUp) || !isfinite(minUp))
+  {
+    std::cout << "ERROR: getHeight() computed a bad value. maxUp=" << maxUp
+              << ", minUp=" << minUp << ". Exiting..." << std::endl;
+    throw std::runtime_error("getHeight() computed a height of 0");
+  }
   if (isfinite(maxUp) && isfinite(minUp))
   {
     height = maxUp - minUp;