From 62ee52cf00c39b50eeea3570f79623afa2225fc8 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@openrobotics.org>
Date: Tue, 23 Apr 2024 19:57:12 -0700
Subject: [PATCH] bullet-featurestore: Enable auto deactivation (#630)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
---
 bullet-featherstone/src/FreeGroupFeatures.cc  |  3 +++
 bullet-featherstone/src/JointFeatures.cc      |  6 +++++-
 bullet-featherstone/src/LinkFeatures.cc       |  2 ++
 bullet-featherstone/src/SimulationFeatures.cc | 19 -------------------
 4 files changed, 10 insertions(+), 20 deletions(-)

diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc
index 4fd79f57f..3604f27ca 100644
--- a/bullet-featherstone/src/FreeGroupFeatures.cc
+++ b/bullet-featherstone/src/FreeGroupFeatures.cc
@@ -69,6 +69,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
   if (model)
   {
     model->body->setBaseOmega(convertVec(_angularVelocity));
+    model->body->wakeUp();
   }
 }
 
@@ -82,6 +83,7 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
   if (model)
   {
     model->body->setBaseVel(convertVec(_linearVelocity));
+    model->body->wakeUp();
   }
 }
 
@@ -95,6 +97,7 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
   {
     model->body->setBaseWorldTransform(
         convertTf(_pose * model->baseInertiaToLinkFrame.inverse()));
+    model->body->wakeUp();
   }
 }
 
diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc
index 980313a7b..588a26a6a 100644
--- a/bullet-featherstone/src/JointFeatures.cc
+++ b/bullet-featherstone/src/JointFeatures.cc
@@ -112,6 +112,7 @@ void JointFeatures::SetJointPosition(
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
   model->body->getJointPosMultiDof(identifier->indexInBtModel)[_dof] =
       static_cast<btScalar>(_value);
+  model->body->wakeUp();
 }
 
 /////////////////////////////////////////////////
@@ -126,6 +127,7 @@ void JointFeatures::SetJointVelocity(
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
   model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] =
       static_cast<btScalar>(_value);
+  model->body->wakeUp();
 }
 
 /////////////////////////////////////////////////
@@ -156,6 +158,7 @@ void JointFeatures::SetJointForce(
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
   model->body->getJointTorqueMultiDof(
     identifier->indexInBtModel)[_dof] = static_cast<btScalar>(_value);
+  model->body->wakeUp();
 }
 
 /////////////////////////////////////////////////
@@ -282,9 +285,9 @@ void JointFeatures::SetJointVelocityCommand(
     return;
   }
 
+  auto modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
   if (!jointInfo->motor)
   {
-    auto modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
     jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
       modelInfo->body.get(),
       std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
@@ -296,6 +299,7 @@ void JointFeatures::SetJointVelocityCommand(
   }
 
   jointInfo->motor->setVelocityTarget(static_cast<btScalar>(_value));
+  modelInfo->body->wakeUp();
 }
 
 /////////////////////////////////////////////////
diff --git a/bullet-featherstone/src/LinkFeatures.cc b/bullet-featherstone/src/LinkFeatures.cc
index b8dc44356..29e227d7d 100644
--- a/bullet-featherstone/src/LinkFeatures.cc
+++ b/bullet-featherstone/src/LinkFeatures.cc
@@ -50,6 +50,7 @@ void LinkFeatures::AddLinkExternalForceInWorld(
     model->body->addBaseForce(F);
     model->body->addBaseTorque(relPosWorld.cross(F));
   }
+  model->body->wakeUp();
 }
 
 /////////////////////////////////////////////////
@@ -73,6 +74,7 @@ void LinkFeatures::AddLinkExternalTorqueInWorld(
     btVector3 torqueWorld = model->body->getBaseWorldTransform().getBasis() * T;
     model->body->addBaseTorque(torqueWorld);
   }
+  model->body->wakeUp();
 }
 
 }
diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc
index a34a6741c..a32d93d2c 100644
--- a/bullet-featherstone/src/SimulationFeatures.cc
+++ b/bullet-featherstone/src/SimulationFeatures.cc
@@ -46,25 +46,6 @@ void SimulationFeatures::WorldForwardStep(
   worldInfo->world->stepSimulation(static_cast<btScalar>(this->stepSize), 1,
                                    static_cast<btScalar>(this->stepSize));
 
-  for (auto & m : this->models)
-  {
-    if (m.second->body)
-    {
-      m.second->body->checkMotionAndSleepIfRequired(
-          static_cast<btScalar>(this->stepSize));
-      btMultiBodyLinkCollider* col = m.second->body->getBaseCollider();
-      if (col && col->getActivationState() != DISABLE_DEACTIVATION)
-        col->setActivationState(ACTIVE_TAG);
-
-      for (int b = 0; b < m.second->body->getNumLinks(); b++)
-      {
-        col =  m.second->body->getLink(b).m_collider;
-        if (col && col->getActivationState() != DISABLE_DEACTIVATION)
-          col->setActivationState(ACTIVE_TAG);
-      }
-    }
-  }
-
   this->WriteRequiredData(_h);
   this->Write(_h.Get<ChangedWorldPoses>());
 }