diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_extended_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_extended_env.py
index 1449441dd..eda9b3195 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_extended_env.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_extended_env.py
@@ -122,7 +122,7 @@ class MinitaurExtendedEnv(MinitaurReactiveEnv):
     leg_model = []
     if self._include_leg_model:
       raw_motor_angles = self.minitaur.GetMotorAngles()
-      leg_model = self._convert_to_leg_model(raw_motor_angles)
+      leg_model = self.convert_to_leg_model(raw_motor_angles)
 
     observation_list = (
         [parent_observation] + history_states + history_actions +
@@ -185,7 +185,7 @@ class MinitaurExtendedEnv(MinitaurReactiveEnv):
     if self._never_terminate:
       return False
 
-    leg_model = self._convert_to_leg_model(self.minitaur.GetMotorAngles())
+    leg_model = self.convert_to_leg_model(self.minitaur.GetMotorAngles())
     swing0 = leg_model[0]
     swing1 = leg_model[2]
     maximum_swing_angle = 0.8
diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h
index cbe959bdc..f607b8662 100644
--- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h
+++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h
@@ -176,24 +176,30 @@ public:
                 btMatrix3x3 P;
                 firstPiola(psb->m_tetraScratches[j],P);
 #if USE_SVD
-                btMatrix3x3 U, V;
-                btVector3 sigma;
-                singularValueDecomposition(P, U, sigma, V);
                 if (max_p > 0)
                 {
-                    sigma[0] = btMin(sigma[0], max_p);
-                    sigma[1] = btMin(sigma[1], max_p);
-                    sigma[2] = btMin(sigma[2], max_p);
-                    sigma[0] = btMax(sigma[0], -max_p);
-                    sigma[1] = btMax(sigma[1], -max_p);
-                    sigma[2] = btMax(sigma[2], -max_p);
+                    // since we want to clamp the principal stress to max_p, we only need to
+                    // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
+                    btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
+                    if (trPTP > max_p * max_p)
+                    {
+                        btMatrix3x3 U, V;
+                        btVector3 sigma;
+                        singularValueDecomposition(P, U, sigma, V);
+                        sigma[0] = btMin(sigma[0], max_p);
+                        sigma[1] = btMin(sigma[1], max_p);
+                        sigma[2] = btMin(sigma[2], max_p);
+                        sigma[0] = btMax(sigma[0], -max_p);
+                        sigma[1] = btMax(sigma[1], -max_p);
+                        sigma[2] = btMax(sigma[2], -max_p);
+                        btMatrix3x3 Sigma;
+                        Sigma.setIdentity();
+                        Sigma[0][0] = sigma[0];
+                        Sigma[1][1] = sigma[1];
+                        Sigma[2][2] = sigma[2];
+                        P = U * Sigma * V.transpose();
+                    }
                 }
-                btMatrix3x3 Sigma;
-                Sigma.setIdentity();
-                Sigma[0][0] = sigma[0];
-                Sigma[1][1] = sigma[1];
-                Sigma[2][2] = sigma[2];
-                P = U * Sigma * V.transpose();
 #endif
 //                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();