From 5b74d1fdeeba3afd2f6bbbddf76689998992cc72 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@openrobotics.org>
Date: Thu, 7 Dec 2023 21:35:16 +0000
Subject: [PATCH] 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 <ichen@openrobotics.org>
---
 bullet-featherstone/src/SDFFeatures.cc | 21 ++++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
index 01854690e..da528ae35 100644
--- a/bullet-featherstone/src/SDFFeatures.cc
+++ b/bullet-featherstone/src/SDFFeatures.cc
@@ -794,7 +794,7 @@ 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())
   {
@@ -802,27 +802,32 @@ bool SDFFeatures::AddSdfCollision(
     {
       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),