Misc stability improvements

keenon · Oct 29, 2022 · f082514 · f082514
1 parent 16cb655
commit f082514
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 10 deletions.
diff --git a/VERSION.txt b/VERSION.txt
@@ -1 +1 @@
-0.8.35.2
+0.8.36
diff --git 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
@@ -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;