From f082514569c41ab39d2295d914d29a5580b42867 Mon Sep 17 00:00:00 2001
From: Keenon Werling <keenonwerling@gmail.com>
Date: Fri, 28 Oct 2022 17:12:55 -0700
Subject: [PATCH] Misc stability improvements

---
 VERSION.txt                                        |  2 +-
 dart/dynamics/ConstantCurveIncompressibleJoint.cpp | 10 +++++-----
 dart/dynamics/Inertia.cpp                          |  8 ++++----
 3 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/VERSION.txt b/VERSION.txt
index 2bbc5f9b7..98bb028db 100644
--- a/VERSION.txt
+++ b/VERSION.txt
@@ -1 +1 @@
-0.8.35.2
\ No newline at end of file
+0.8.36
\ No newline at end of file
diff --git a/dart/dynamics/ConstantCurveIncompressibleJoint.cpp b/dart/dynamics/ConstantCurveIncompressibleJoint.cpp
index b03b9ffdf..f66bfb2fc 100644
--- a/dart/dynamics/ConstantCurveIncompressibleJoint.cpp
+++ b/dart/dynamics/ConstantCurveIncompressibleJoint.cpp
@@ -139,7 +139,7 @@ Eigen::Isometry3s ConstantCurveIncompressibleJoint::getRelativeTransformAt(
 
   s_t sinTheta
       = sqrt(linearAngle(0) * linearAngle(0) + linearAngle(2) * linearAngle(2));
-  if (sinTheta < 0.001)
+  if (sinTheta < 0.001 || sinTheta > 0.999)
   {
     // Near very vertical angles, don't worry about the bend, just approximate
     // with an euler joint
@@ -190,7 +190,7 @@ void ConstantCurveIncompressibleJoint::updateRelativeTransform() const
 
   s_t sinTheta
       = sqrt(linearAngle(0) * linearAngle(0) + linearAngle(2) * linearAngle(2));
-  if (sinTheta < 0.001)
+  if (sinTheta < 0.001 || sinTheta > 0.999)
   {
     // Near very vertical angles, don't worry about the bend, just approximate
     // with an euler joint
@@ -258,7 +258,7 @@ ConstantCurveIncompressibleJoint::getRelativeJacobianStatic(
   s_t sinTheta
       = sqrt(linearAngle(0) * linearAngle(0) + linearAngle(2) * linearAngle(2));
 
-  if (sinTheta < 0.001)
+  if (sinTheta < 0.001 || sinTheta > 0.999)
   {
     // Near very vertical angles, don't worry about the bend, just approximate
     // with an euler joint
@@ -397,7 +397,7 @@ ConstantCurveIncompressibleJoint::getRelativeJacobianDerivWrtPositionStatic(
   const s_t sinTheta
       = sqrt(linearAngle(0) * linearAngle(0) + linearAngle(2) * linearAngle(2));
 
-  if (sinTheta < 0.001)
+  if (sinTheta < 0.001 || sinTheta > 0.999)
   {
     // Near very vertical angles, don't worry about the bend, just approximate
     // with an euler joint
@@ -726,7 +726,7 @@ Eigen::Matrix<s_t, 6, 3> ConstantCurveIncompressibleJoint::
   const s_t sinTheta
       = sqrt(linearAngle(0) * linearAngle(0) + linearAngle(2) * linearAngle(2));
 
-  if (sinTheta < 0.003)
+  if (sinTheta < 0.003 || sinTheta > 0.997)
   {
     // Near very vertical angles, don't worry about the bend, just approximate
     // with an euler joint
diff --git a/dart/dynamics/Inertia.cpp b/dart/dynamics/Inertia.cpp
index e6c55699f..a787b455e 100644
--- a/dart/dynamics/Inertia.cpp
+++ b/dart/dynamics/Inertia.cpp
@@ -1140,11 +1140,11 @@ Eigen::Vector6s Inertia::computeDimsAndEuler(
   const s_t yy = principalInertia(1);
   const s_t zz = principalInertia(2);
   assert(mass != 0);
-  assert(6 * (xx + zz - yy) / mass > 0);
+  const s_t MAX_DIM = 0.1;
   Eigen::Vector3s dim = Eigen::Vector3s(
-      sqrt(6 * (yy + zz - xx) / mass),
-      sqrt(6 * (xx + zz - yy) / mass),
-      sqrt(6 * (xx + yy - zz) / mass));
+      (yy + zz - xx) > 0 ? sqrt(6 * (yy + zz - xx) / mass) : MAX_DIM,
+      (xx + zz - yy) > 0 ? sqrt(6 * (xx + zz - yy) / mass) : MAX_DIM,
+      (xx + yy - zz) > 0 ? sqrt(6 * (xx + yy - zz) / mass) : MAX_DIM);
   assert(!dim.hasNaN());
 
   Eigen::Vector6s result;