bullet-featherstone: Set collision spinning friction (#579)

Set collision spinning friction using on sdf's torsional coefficient value. If not specified in sdf, the default value of 1.0 is used. When 2 boxes penetrate, bullet pushes them apart. Without the spinning friction set, the boxes will continue to spin indefinitely or until it collides with another object. Setting the spinning friction prevents this from happening. Signed-off-by: Ian Chen <[email protected]>
gazebosim · Dec 7, 2023 · 5b74d1f · 5b74d1f
1 parent af58fa5
commit 5b74d1f
Showing 1 changed file with 14 additions and 7 deletions.
diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
@@ -794,35 +794,40 @@ bool SDFFeatures::AddSdfCollision(
   double mu = 1.0;
   double mu2 = 1.0;
   double restitution = 0.0;
-
+  double torsionalCoefficient = 1.0;
   double rollingFriction = 0.0;
   if (const auto *surface = _collision.Surface())
   {
     if (const auto *friction = surface->Friction())
     {
       if (const auto frictionElement = friction->Element())
       {
-        if (const auto bullet = frictionElement->GetElement("bullet"))
+        if (const auto bullet = frictionElement->FindElement("bullet"))
         {
-          if (const auto f1 = bullet->GetElement("friction"))
+          if (const auto f1 = bullet->FindElement("friction"))
             mu = f1->Get<double>();
 
-          if (const auto f2 = bullet->GetElement("friction2"))
+          if (const auto f2 = bullet->FindElement("friction2"))
             mu2 = f2->Get<double>();
 
           // What is fdir1 for in the SDF's <bullet> spec?
 
-          if (const auto rolling = bullet->GetElement("rolling_friction"))
+          if (const auto rolling = bullet->FindElement("rolling_friction"))
             rollingFriction = rolling->Get<double>();
         }
+        if (const auto torsional = frictionElement->FindElement("torsional"))
+        {
+          if (const auto coefficient = torsional->FindElement("coefficient"))
+            torsionalCoefficient = coefficient->Get<double>();
+        }
       }
     }
 
     if (const auto surfaceElement = surface->Element())
     {
-      if (const auto bounce = surfaceElement->GetElement("bounce"))
+      if (const auto bounce = surfaceElement->FindElement("bounce"))
       {
-        if (const auto r = bounce->GetElement("restitution_coefficient"))
+        if (const auto r = bounce->FindElement("restitution_coefficient"))
           restitution = r->Get<double>();
       }
     }
@@ -873,6 +878,8 @@ bool SDFFeatures::AddSdfCollision(
       linkInfo->collider->setRestitution(static_cast<btScalar>(restitution));
       linkInfo->collider->setRollingFriction(
         static_cast<btScalar>(rollingFriction));
+      linkInfo->collider->setSpinningFriction(
+        static_cast<btScalar>(torsionalCoefficient));
       linkInfo->collider->setFriction(static_cast<btScalar>(mu));
       linkInfo->collider->setAnisotropicFriction(
         btVector3(static_cast<btScalar>(mu), static_cast<btScalar>(mu2), 1),