From 86b2da3c6c22767452299b6b05f9dc65b5fd3f01 Mon Sep 17 00:00:00 2001
From: "Addisu Z. Taddese" <addisu@openrobotics.org>
Date: Thu, 19 Dec 2024 21:32:15 +0100
Subject: [PATCH] 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>
(cherry picked from commit a369e1801688af5c90876873a3509bd65003f110)
---
 dartsim/src/Base.hh               |  1 +
 dartsim/src/SimulationFeatures.cc | 33 +++++++++++++++++++++++++++++++
 2 files changed, 34 insertions(+)

diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh
index 684d570c1..c37f1709a 100644
--- a/dartsim/src/Base.hh
+++ b/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
diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc
index 9475ef984..3623abe3c 100644
--- a/dartsim/src/SimulationFeatures.cc
+++ b/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