diff --git a/dart/biomechanics/IKInitializer.cpp b/dart/biomechanics/IKInitializer.cpp index e2316df7f..0696aafe9 100644 --- a/dart/biomechanics/IKInitializer.cpp +++ b/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 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); } } diff --git a/dart/biomechanics/SubjectOnDisk.cpp b/dart/biomechanics/SubjectOnDisk.cpp index 077ceab40..769b36f79 100644 --- a/dart/biomechanics/SubjectOnDisk.cpp +++ b/dart/biomechanics/SubjectOnDisk.cpp @@ -782,6 +782,16 @@ std::shared_ptr 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()); diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 47b641ef3..2448d14d1 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/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." diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 1dcf038d8..334da76ef 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/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(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;