bullet-featherstone: fix setting angular velocity

* Use btMultiBody::setBaseOmega API * Fix null pointer check Signed-off-by: Steve Peters <[email protected]>
gazebosim · Nov 9, 2023 · 28a0d15 · 28a0d15
1 parent a4359ef
commit 28a0d15
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 24 deletions.
diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc
@@ -68,29 +68,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
 
   if (model != nullptr)
   {
-    // Set angular velocity the each one of the joints of the model
-    for (const auto& jointID : model->jointEntityIds)
-    {
-      auto jointInfo = this->joints[jointID];
-      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,
-          0,
-          static_cast<btScalar>(0),
-          static_cast<btScalar>(jointInfo->effort));
-        auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
-        world->world->addMultiBodyConstraint(jointInfo->motor.get());
-      }
-
-      if (jointInfo->motor)
-      {
-        jointInfo->motor->setVelocityTarget(
-            static_cast<btScalar>(_angularVelocity[2]));
-      }
-    }
+    model->body->setBaseOmega(convertVec(_angularVelocity));
   }
 }
 

diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc
@@ -104,7 +104,7 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
       }
     }
 
-    if (model->body == nullptr)
+    if (!model || model->body == nullptr)
       model = this->FrameInterface<ModelInfo>(_id);
   }
-Original file line number
+Diff line change
@@ Expand Up / @@ -104,7 +104,7 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( @@
           }
         }
-        if (model->body == nullptr)
+        if (!model || model->body == nullptr)
           model = this->FrameInterface<ModelInfo>(_id);
       }
@@ Expand Down @@