From 28a0d15e56be9ba8a7700b1f6ec73820bf1c7ab0 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 9 Nov 2023 04:39:13 -0800
Subject: [PATCH] bullet-featherstone: fix setting angular velocity

* Use btMultiBody::setBaseOmega API
* Fix null pointer check

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 bullet-featherstone/src/FreeGroupFeatures.cc  | 24 +------------------
 bullet-featherstone/src/KinematicsFeatures.cc |  2 +-
 2 files changed, 2 insertions(+), 24 deletions(-)

diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc
index 5fd18e9f3..c557d6465 100644
--- 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
index 74f16776d..85341ce6a 100644
--- 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);
   }