bullet-featurestore: Enable auto deactivation (#630)

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Apr 24, 2024 · 62ee52c · 62ee52c
1 parent c96ce6e
commit 62ee52c
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 20 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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>());
 }