Merge pull request #3614 from opensim-org/constant_curvature_joint_fix

Use `OPENSIM_ASSERT` in `ConstantCurvatureJoint::ConstantCurvatureJointImpl`

Author: nickbianco

OpenSim/Simulation/SimbodyEngine/ConstantCurvatureJoint.cpp

@@ -56,7 +56,7 @@ Vec3 OpenSim::ConstantCurvatureJoint::clamp(const SimTK::Vec3& q) {
                 "to unphysical behavior. Please adjust your model "
                 "or simulation to avoid this state.",
                 pos(0), bound));
-        OPENSIM_ASSERT_FRMOBJ(false);
+        OPENSIM_ASSERT(false);
         pos(0) = bound;
     }
     if (pos(0) < -bound) {
@@ -67,7 +67,7 @@ Vec3 OpenSim::ConstantCurvatureJoint::clamp(const SimTK::Vec3& q) {
                 "to unphysical behavior. Please adjust your model "
                 "or simulation to avoid this state.",
                 pos(0), -bound));
-        OPENSIM_ASSERT_FRMOBJ(false);
+        OPENSIM_ASSERT(false);
         pos(0) = -bound;
     }
     if (pos(1) > bound) {
@@ -78,7 +78,7 @@ Vec3 OpenSim::ConstantCurvatureJoint::clamp(const SimTK::Vec3& q) {
                 "to unphysical behavior. Please adjust your model "
                 "or simulation to avoid this state.",
                 pos(1), bound));
-        OPENSIM_ASSERT_FRMOBJ(false);
+        OPENSIM_ASSERT(false);
         pos(1) = bound;
     }
     if (pos(1) < -bound) {
@@ -89,7 +89,7 @@ Vec3 OpenSim::ConstantCurvatureJoint::clamp(const SimTK::Vec3& q) {
                 "to unphysical behavior. Please adjust your model "
                 "or simulation to avoid this state.",
                 pos(1), -bound));
-        OPENSIM_ASSERT_FRMOBJ(false);
+        OPENSIM_ASSERT(false);
         pos(1) = -bound;
     }
     if (pos(2) > bound) {
@@ -100,7 +100,7 @@ Vec3 OpenSim::ConstantCurvatureJoint::clamp(const SimTK::Vec3& q) {
                 "to unphysical behavior. Please adjust your model "
                 "or simulation to avoid this state.",
                 pos(2), bound));
-        OPENSIM_ASSERT_FRMOBJ(false);
+        OPENSIM_ASSERT(false);
         pos(2) = bound;
     }
     if (pos(2) < -bound) {
@@ -111,7 +111,7 @@ Vec3 OpenSim::ConstantCurvatureJoint::clamp(const SimTK::Vec3& q) {
                 "to unphysical behavior. Please adjust your model "
                 "or simulation to avoid this state.",
                 pos(2), -bound));
-        OPENSIM_ASSERT_FRMOBJ(false);
+        OPENSIM_ASSERT(false);
         pos(2) = -bound;
     }
     return pos;
@@ -205,7 +205,7 @@ Mat33 OpenSim::ConstantCurvatureJoint::eulerXZYToMatrixGrad(
         ret(1, 2) = -(-sy) * sx + cx * (cy)*sz;
         ret(2, 2) = cx * (-sy) + sx * (cy)*sz;
     } else {
-        OPENSIM_ASSERT_FRMOBJ(false);
+        OPENSIM_ASSERT(false);
     }
 
     return ret;
@@ -247,7 +247,7 @@ Mat63 OpenSim::ConstantCurvatureJoint::getEulerJacobian(const Vec3& q) {
 
 Mat63 OpenSim::ConstantCurvatureJoint::getEulerJacobianDerivWrtPos(
         const Vec3& q, int index) {
-    OPENSIM_ASSERT_FRMOBJ(index < 3);
+    OPENSIM_ASSERT(index < 3);
 
     Mat63 DJ_Dq;
     DJ_Dq.setToZero();
@@ -678,10 +678,10 @@ class ConstantCurvatureJointImpl
                 OpenSim::ConstantCurvatureJoint::clamp(q + neutralPos), length);
         Vec6 result = J * Vec3(u[0], u[1], u[2]);
 
-        OPENSIM_ASSERT_FRMOBJ(!q.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!J.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!J.isInf());
-        OPENSIM_ASSERT_FRMOBJ(!result.isNaN());
+        OPENSIM_ASSERT(!q.isNaN());
+        OPENSIM_ASSERT(!J.isNaN());
+        OPENSIM_ASSERT(!J.isInf());
+        OPENSIM_ASSERT(!result.isNaN());
 
         return SpatialVec(result.getSubVec<3>(0), result.getSubVec<3>(3));
     }
@@ -699,10 +699,10 @@ class ConstantCurvatureJointImpl
 
         Vec3 result = J.transpose() * rawSpatial;
 
-        OPENSIM_ASSERT_FRMOBJ(!q.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!J.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!J.isInf());
-        OPENSIM_ASSERT_FRMOBJ(!result.isNaN());
+        OPENSIM_ASSERT(!q.isNaN());
+        OPENSIM_ASSERT(!J.isNaN());
+        OPENSIM_ASSERT(!J.isInf());
+        OPENSIM_ASSERT(!result.isNaN());
 
         Vec3::updAs(f) = result;
     }
@@ -722,11 +722,11 @@ class ConstantCurvatureJointImpl
                         dq, length);
         Vec6 result = dJ * Vec3(u[0], u[1], u[2]);
 
-        OPENSIM_ASSERT_FRMOBJ(!q.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!dq.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!dJ.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!dJ.isInf());
-        OPENSIM_ASSERT_FRMOBJ(!result.isNaN());
+        OPENSIM_ASSERT(!q.isNaN());
+        OPENSIM_ASSERT(!dq.isNaN());
+        OPENSIM_ASSERT(!dJ.isNaN());
+        OPENSIM_ASSERT(!dJ.isInf());
+        OPENSIM_ASSERT(!result.isNaN());
 
         return SpatialVec(result.getSubVec<3>(0), result.getSubVec<3>(3));
     }
@@ -748,24 +748,24 @@ class ConstantCurvatureJointImpl
 
         Vec3 result = dJ.transpose() * rawSpatial;
 
-        OPENSIM_ASSERT_FRMOBJ(!q.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!dq.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!dJ.isNaN());
-        OPENSIM_ASSERT_FRMOBJ(!dJ.isInf());
-        OPENSIM_ASSERT_FRMOBJ(!result.isNaN());
+        OPENSIM_ASSERT(!q.isNaN());
+        OPENSIM_ASSERT(!dq.isNaN());
+        OPENSIM_ASSERT(!dJ.isNaN());
+        OPENSIM_ASSERT(!dJ.isInf());
+        OPENSIM_ASSERT(!result.isNaN());
 
         Vec3::updAs(f) = result;
     }
 
     void setQToFitTransform(
             const State&, const Transform& X_FM, int nq, Real* q) const {
-        OPENSIM_ASSERT_FRMOBJ(false);
+        OPENSIM_ASSERT(false);
         Vec3::updAs(q) = X_FM.p();
     }
 
     void setUToFitVelocity(
             const State&, const SpatialVec& V_FM, int nu, Real* u) const {
-        OPENSIM_ASSERT_FRMOBJ(false);
+        OPENSIM_ASSERT(false);
         Vec3::updAs(u) = V_FM[1];
     }