Misc fixes and minor AddB support features

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;
 }
 
@@ -9526,6 +9544,36 @@ std::shared_ptr<DynamicsInitialization> DynamicsFitter::createInitialization(
     init->regularizeMarkerOffsetsTo[pair.first] = pair.second;
   }
 
+  // Create the joint data
+  init->joints.clear();
+  for (int trial = 0; trial < poseTrials.size(); trial++)
+  {
+    init->joints.emplace_back();
+    for (int i = 0; i < skel->getNumJoints(); i++)
+    {
+      init->joints.back().push_back(skel->getJoint(i));
+    }
+    init->jointsAdjacentMarkers.emplace_back();
+    init->jointWeights.push_back(Eigen::VectorXs::Ones(skel->getNumJoints()));
+    init->axisWeights.push_back(Eigen::VectorXs::Zero(skel->getNumJoints()));
+  }
+
+  for (int trial = 0; trial < poseTrials.size(); trial++)
+  {
+    Eigen::MatrixXs& poses = poseTrials[trial];
+    Eigen::MatrixXs jointCenters
+        = Eigen::MatrixXs::Zero(3 * skel->getNumJoints(), poses.cols());
+    Eigen::MatrixXs jointAxis
+        = Eigen::MatrixXs::Zero(6 * skel->getNumJoints(), poses.cols());
+    for (int t = 0; t < poses.cols(); t++)
+    {
+      skel->setPositions(poses.col(t));
+      jointCenters.col(t) = skel->getJointWorldPositions(skel->getJoints());
+    }
+    init->jointCenters.push_back(jointCenters);
+    init->jointAxis.push_back(jointAxis);
+  }
+
   return init;
 }
 
@@ -9589,6 +9637,10 @@ std::shared_ptr<DynamicsInitialization> DynamicsFitter::createInitialization(
 
   // Copy over the joint data
   init->joints.clear();
+  init->jointWeights.clear();
+  init->axisWeights.clear();
+  init->jointCenters.clear();
+  init->jointAxis.clear();
   for (int trial = 0; trial < kinematicInit.size(); trial++)
   {
     init->joints.emplace_back();
@@ -9606,6 +9658,9 @@ std::shared_ptr<DynamicsInitialization> DynamicsFitter::createInitialization(
 
   for (int trial = 0; trial < init->poseTrials.size(); trial++)
   {
+    assert(
+        kinematicInit.at(trial).jointCenters.rows() / 3
+        == init->joints.at(trial).size());
     init->jointCenters.push_back(kinematicInit.at(trial).jointCenters);
     init->jointAxis.push_back(kinematicInit.at(trial).jointAxis);
   }
@@ -11618,7 +11673,8 @@ bool DynamicsFitter::zeroLinearResidualsOnCOMTrajectory(
     //     unifiedPositions - unifiedGravityOffset);
     Eigen::LeastSquaresConjugateGradient<Eigen::MatrixXs> solver;
     solver.setTolerance(1e-9);
-    solver.setMaxIterations(unifiedLinearMap.rows() * 10);
+    solver.setMaxIterations(
+        std::min<int>((int)(unifiedLinearMap.rows() * 10), 10000));
     solver.compute(unifiedLinearMap);
     Eigen::VectorXs tentativeResult
         = solver.solve(unifiedPositions - unifiedGravityOffset);
@@ -18350,6 +18406,8 @@ void DynamicsFitter::saveDynamicsToGUI(
 
   // Render the joints, if we have them
   int numJoints = init->jointCenters.at(trialIndex).rows() / 3;
+  assert(numJoints == init->jointWeights.at(trialIndex).size());
+  assert(numJoints == init->joints.at(trialIndex).size());
   server.createLayer(
       functionalJointCenterLayerName, functionalJointCenterLayerColor, true);
   for (int i = 0; i < numJoints; i++)

dart/biomechanics/OpenSimParser.cpp

@@ -4851,6 +4851,7 @@ Eigen::Vector3s readAttachedGeometry(
     dynamics::BodyNode* childBody,
     Eigen::Isometry3s relativeT,
     const std::string fileNameForErrorDisplay,
+    OpenSimFile& file,
     const std::string geometryFolder,
     const common::ResourceRetrieverPtr& geometryRetriever,
     bool ignoreGeometry)
@@ -4885,6 +4886,19 @@ Eigen::Vector3s readAttachedGeometry(
       numScales++;
     }
 
+    Eigen::Isometry3s localT = Eigen::Isometry3s::Identity();
+    tinyxml2::XMLElement* transformElem
+        = meshCursor->FirstChildElement("transform");
+    if (transformElem != nullptr)
+    {
+      Eigen::Vector6s transformVec = readVec6(transformElem);
+      localT.linear() = math::eulerXYZToMatrix(transformVec.head<3>());
+      localT.translation() = transformVec.tail<3>();
+    }
+    file.meshMap[mesh_file]
+        = std::make_pair(childBody->getName(), relativeT * localT);
+    file.meshScaleMap[mesh_file] = scale;
+
     if (!ignoreGeometry)
     {
       common::Uri meshUri = common::Uri::createFromRelativeUri(
@@ -4903,16 +4917,6 @@ Eigen::Vector3s readAttachedGeometry(
               = childBody->createShapeNodeWith<dynamics::VisualAspect>(
                   meshShape);
 
-          Eigen::Isometry3s localT = Eigen::Isometry3s::Identity();
-          tinyxml2::XMLElement* transformElem
-              = meshCursor->FirstChildElement("transform");
-          if (transformElem != nullptr)
-          {
-            Eigen::Vector6s transformVec = readVec6(transformElem);
-            localT.linear() = math::eulerXYZToMatrix(transformVec.head<3>());
-            localT.translation() = transformVec.tail<3>();
-          }
-
           meshShapeNode->setRelativeTransform(relativeT * localT);
 
           dynamics::VisualAspect* meshVisualAspect
@@ -4954,6 +4958,7 @@ std::tuple<dynamics::Joint*, dynamics::BodyNode*, Eigen::Vector3s> createJoint(
     Eigen::Isometry3s transformFromParent,
     Eigen::Isometry3s transformFromChild,
     const std::string fileNameForErrorDisplay,
+    OpenSimFile& file,
     const std::string geometryFolder,
     const common::ResourceRetrieverPtr& geometryRetriever,
     bool ignoreGeometry)
@@ -5922,6 +5927,9 @@ std::tuple<dynamics::Joint*, dynamics::BodyNode*, Eigen::Vector3s> createJoint(
             numScalesCounted++;
           }
 
+          file.meshMap[mesh_file] = std::make_pair(bodyName, transform);
+          file.meshScaleMap[mesh_file] = scale;
+
           if (!ignoreGeometry)
           {
             common::Uri meshUri = common::Uri::createFromRelativeUri(
@@ -5989,6 +5997,7 @@ std::tuple<dynamics::Joint*, dynamics::BodyNode*, Eigen::Vector3s> createJoint(
             childBody,
             relativeT,
             fileNameForErrorDisplay,
+            file,
             geometryFolder,
             geometryRetriever,
             ignoreGeometry);
@@ -6007,6 +6016,7 @@ std::tuple<dynamics::Joint*, dynamics::BodyNode*, Eigen::Vector3s> createJoint(
         childBody,
         Eigen::Isometry3s::Identity(),
         fileNameForErrorDisplay,
+        file,
         geometryFolder,
         geometryRetriever,
         ignoreGeometry);
@@ -6195,6 +6205,7 @@ OpenSimFile OpenSimParser::readOsim30(
             transformFromParent,
             transformFromChild,
             fileNameForErrorDisplay,
+            file,
             geometryFolder,
             geometryRetriever,
             ignoreGeometry);
@@ -6390,6 +6401,7 @@ void recursiveCreateJoint(
       joint->fromParent,
       joint->fromChild,
       fileNameForErrorDisplay,
+      file,
       geometryFolder,
       geometryRetriever,
       ignoreGeometry);

dart/biomechanics/OpenSimParser.hpp

@@ -39,6 +39,11 @@ struct OpenSimFile
   // IMU map
   std::map<std::string, std::pair<std::string, Eigen::Isometry3s>> imuMap;
 
+  // Mesh map, so that we can access mesh positioning data even if we
+  // are not loading the geometry.
+  std::map<std::string, std::pair<std::string, Eigen::Isometry3s>> meshMap;
+  std::map<std::string, Eigen::Vector3s> meshScaleMap;
+
   std::vector<std::string> warnings;
   std::vector<std::string> ignoredBodies;
   std::vector<std::pair<std::string, std::string>> jointsDrivenBy;