Prevent crash when objects move to invalid poses (#706)

DART seems to compute invalid poses for certain objects (e.g. Capsule, Ellipse) with large accelerations. The computed poses end up with `nan` values resulting in a crash. This patch detects this condition and reassigns the poses to the last known poses and resets velocities to zero to prevent the crash.

---------

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

Author: azeey

dartsim/src/Base.hh

@@ -97,6 +97,7 @@ struct ModelInfo
   std::vector<std::shared_ptr<LinkInfo>> links {};
   std::vector<std::shared_ptr<JointInfo>> joints {};
   std::vector<std::size_t> nestedModels = {};
+  std::optional<Eigen::VectorXd> lastGoodPositions = {};
 };
 
 struct ShapeInfo

dartsim/src/SimulationFeatures.cc

@@ -15,8 +15,10 @@
  *
 */
 
+#include <Eigen/Geometry>
 #include <limits>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -30,6 +32,7 @@
 #include <dart/constraint/ContactSurface.hpp>
 #endif
 
+#include <gz/common/Console.hh>
 #include <gz/common/Profiler.hh>
 
 #include <gz/math/Pose3.hh>
@@ -106,6 +109,36 @@ void SimulationFeatures::WorldForwardStep(
 
   // TODO(MXG): Parse input
   world->step();
+
+  for (auto &[ignore, modelInfo] : this->models.idToObject)
+  {
+    const Eigen::VectorXd &positions = modelInfo->model->getPositions();
+    if (positions.hasNaN())
+    {
+      std::stringstream ss;
+      ss << "Some links in model '" << modelInfo->localName
+         << "' have invalid poses. ";
+      if (modelInfo->lastGoodPositions)
+      {
+        ss << "Resetting to last known poses.";
+        modelInfo->model->setPositions(*modelInfo->lastGoodPositions);
+      }
+      else
+      {
+        ss << "Resetting to zero poses.";
+        modelInfo->model->resetPositions();
+      }
+      gzerr << ss.str()
+            << " Also resetting velocities and accelerations to zero."
+            << std::endl;
+      modelInfo->model->resetVelocities();
+      modelInfo->model->resetAccelerations();
+    }
+    else
+    {
+      modelInfo->lastGoodPositions = positions;
+    }
+  }
   this->WriteRequiredData(_h);
   this->Write(_h.Get<ChangedWorldPoses>());
   // TODO(MXG): Fill in state