Fix bullet double to float conversion compiler warnings

bullet-featherstone/src/Base.hh

@@ -69,9 +69,9 @@ struct WorldInfo
   std::unique_ptr<btMultiBodyConstraintSolver> solver;
   std::unique_ptr<btMultiBodyDynamicsWorld> world;
 
-  std::unordered_map<std::size_t, std::size_t> modelIndexToEntityId;
+  std::unordered_map<int, std::size_t> modelIndexToEntityId;
   std::unordered_map<std::string, std::size_t> modelNameToEntityId;
-  std::size_t nextModelIndex = 0;
+  int nextModelIndex = 0;
 
   explicit WorldInfo(std::string name);
 };
@@ -80,7 +80,7 @@ struct ModelInfo
 {
   std::string name;
   Identity world;
-  std::size_t indexInWorld;
+  int indexInWorld;
   Eigen::Isometry3d baseInertiaToLinkFrame;
   std::unique_ptr<btMultiBody> body;
 
@@ -112,7 +112,7 @@ struct ModelInfo
 struct LinkInfo
 {
   std::string name;
-  std::optional<std::size_t> indexInModel;
+  std::optional<int> indexInModel;
   Identity model;
   Eigen::Isometry3d inertiaToLinkFrame;
   std::unique_ptr<btMultiBodyLinkCollider> collider = nullptr;
@@ -127,12 +127,12 @@ struct CollisionInfo
   std::unique_ptr<btCollisionShape> collider;
   Identity link;
   Eigen::Isometry3d linkToCollision;
-  std::size_t indexInLink = 0;
+  int indexInLink = 0;
 };
 
 struct InternalJoint
 {
-  std::size_t indexInBtModel;
+  int indexInBtModel;
 };
 
 struct RootJoint {};
@@ -165,21 +165,31 @@ struct JointInfo
   // This field gets set by AddJoint
   std::size_t indexInGzModel = 0;
   btMultiBodyJointMotor* motor = nullptr;
-  double effort = 0;
+  btScalar effort = 0;
 
   std::shared_ptr<btMultiBodyFixedConstraint> fixedContraint = nullptr;
 };
 
 inline btMatrix3x3 convertMat(const Eigen::Matrix3d& mat)
 {
-  return btMatrix3x3(mat(0, 0), mat(0, 1), mat(0, 2),
-                     mat(1, 0), mat(1, 1), mat(1, 2),
-                     mat(2, 0), mat(2, 1), mat(2, 2));
+  return btMatrix3x3(
+      static_cast<btScalar>(mat(0, 0)), static_cast<btScalar>(mat(0, 1)),
+      static_cast<btScalar>(mat(0, 2)), static_cast<btScalar>(mat(1, 0)),
+      static_cast<btScalar>(mat(1, 1)), static_cast<btScalar>(mat(1, 2)),
+      static_cast<btScalar>(mat(2, 0)), static_cast<btScalar>(mat(2, 1)),
+      static_cast<btScalar>(mat(2, 2)));
 }
 
 inline btVector3 convertVec(const Eigen::Vector3d& vec)
 {
-  return btVector3(vec(0), vec(1), vec(2));
+  return btVector3(static_cast<btScalar>(vec(0)), static_cast<btScalar>(vec(1)),
+                   static_cast<btScalar>(vec(2)));
+}
+
+inline btVector3 convertVec(const math::Vector3d& vec)
+{
+  return btVector3(static_cast<btScalar>(vec[0]), static_cast<btScalar>(vec[1]),
+                   static_cast<btScalar>(vec[2]));
 }
 
 inline btTransform convertTf(const Eigen::Isometry3d& tf)
@@ -215,7 +225,7 @@ inline Eigen::Isometry3d convert(const btTransform& tf)
 
 inline btTransform GetWorldTransformOfLinkInertiaFrame(
     const btMultiBody &body,
-    const std::size_t linkIndexInModel)
+    const int linkIndexInModel)
 {
   const auto p = body.localPosToWorld(
     linkIndexInModel, btVector3(0, 0, 0));
@@ -292,7 +302,8 @@ class Base : public Implements3d<FeatureList<Feature>>
     if (link->indexInModel.has_value())
     {
       // We expect the links to be added in order
-      assert(*link->indexInModel+1 == model->linkEntityIds.size());
+      assert(static_cast<std::size_t>(*link->indexInModel + 1) ==
+             model->linkEntityIds.size());
     }
     else
     {
@@ -312,7 +323,7 @@ class Base : public Implements3d<FeatureList<Feature>>
    auto collision = std::make_shared<CollisionInfo>(std::move(_collisionInfo));
    this->collisions[id] = collision;
    auto *link = this->ReferenceInterface<LinkInfo>(_collisionInfo.link);
-   collision->indexInLink = link->collisionEntityIds.size();
+   collision->indexInLink = static_cast<int>(link->collisionEntityIds.size());
    link->collisionEntityIds.push_back(id);
    link->collisionNameToEntityId[collision->name] = id;
 

bullet-featherstone/src/EntityManagementFeatures.cc

@@ -76,7 +76,7 @@ std::size_t EntityManagementFeatures::GetLinkIndex(
   // the rest up by one when providing an index to gazebo
   const auto index = this->ReferenceInterface<LinkInfo>(_linkID)->indexInModel;
   if (index.has_value())
-    return *index+1;
+    return static_cast<std::size_t>(*index+1);
 
   return 0;
 }
@@ -183,7 +183,8 @@ const std::string &EntityManagementFeatures::GetShapeName(
 std::size_t EntityManagementFeatures::GetShapeIndex(
   const Identity &_shapeID) const
 {
-  return this->ReferenceInterface<CollisionInfo>(_shapeID)->indexInLink;
+  return static_cast<std::size_t>(
+      this->ReferenceInterface<CollisionInfo>(_shapeID)->indexInLink);
 }
 
 /////////////////////////////////////////////////
@@ -259,7 +260,8 @@ bool EntityManagementFeatures::RemoveModelByIndex(
     const Identity & _worldID, std::size_t _modelIndex)
 {
   auto *world = this->ReferenceInterface<WorldInfo>(_worldID);
-  const auto it = world->modelIndexToEntityId.find(_modelIndex);
+  const auto it =
+      world->modelIndexToEntityId.find(static_cast<int>(_modelIndex));
   if (it == world->modelIndexToEntityId.end())
     return false;
 
@@ -367,7 +369,8 @@ Identity EntityManagementFeatures::GetModel(
     const Identity &_worldID, std::size_t _modelIndex) const
 {
   const auto *world = this->ReferenceInterface<WorldInfo>(_worldID);
-  const auto it = world->modelIndexToEntityId.find(_modelIndex);
+  const auto it =
+      world->modelIndexToEntityId.find(static_cast<int>(_modelIndex));
   if (it == world->modelIndexToEntityId.end())
     return this->GenerateInvalidId();
 
@@ -401,7 +404,7 @@ std::size_t EntityManagementFeatures::GetModelIndex(
   // the rest up by one when providing an index to gazebo
   const auto index = this->ReferenceInterface<ModelInfo>(
     _modelID)->indexInWorld;
-  return index+1;
+  return static_cast<std::size_t>(index + 1);
 }
 
 /////////////////////////////////////////////////

bullet-featherstone/src/FreeGroupFeatures.cc

@@ -84,7 +84,10 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
       }
 
       if (jointInfo->motor)
-        jointInfo->motor->setVelocityTarget(_angularVelocity[2]);
+      {
+        jointInfo->motor->setVelocityTarget(
+            static_cast<btScalar>(_angularVelocity[2]));
+      }
     }
   }
 }

bullet-featherstone/src/JointFeatures.cc

@@ -109,7 +109,8 @@ void JointFeatures::SetJointPosition(
     return;
 
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
-  model->body->getJointPosMultiDof(identifier->indexInBtModel)[_dof] = _value;
+  model->body->getJointPosMultiDof(identifier->indexInBtModel)[_dof] =
+      static_cast<btScalar>(_value);
 }
 
 /////////////////////////////////////////////////
@@ -122,7 +123,8 @@ void JointFeatures::SetJointVelocity(
     return;
 
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
-  model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] = _value;
+  model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] =
+      static_cast<btScalar>(_value);
 }
 
 /////////////////////////////////////////////////
@@ -152,7 +154,7 @@ void JointFeatures::SetJointForce(
 
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
   model->body->getJointTorqueMultiDof(
-    identifier->indexInBtModel)[_dof] = _value;
+    identifier->indexInBtModel)[_dof] = static_cast<btScalar>(_value);
 }
 
 /////////////////////////////////////////////////
@@ -168,7 +170,8 @@ std::size_t JointFeatures::GetJointDegreesOfFreedom(const Identity &_id) const
   }
 
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
-  return model->body->getLink(identifier->indexInBtModel).m_dofCount;
+  return static_cast<std::size_t>(
+      model->body->getLink(identifier->indexInBtModel).m_dofCount);
 }
 
 /////////////////////////////////////////////////
@@ -291,7 +294,7 @@ void JointFeatures::SetJointVelocityCommand(
     world->world->addMultiBodyConstraint(jointInfo->motor);
   }
 
-  jointInfo->motor->setVelocityTarget(_value);
+  jointInfo->motor->setVelocityTarget(static_cast<btScalar>(_value));
 }
 
 /////////////////////////////////////////////////
@@ -363,10 +366,7 @@ void JointFeatures::SetJointTransformFromParent(
   if (jointInfo->fixedContraint)
   {
       jointInfo->fixedContraint->setPivotInA(
-        btVector3(
-          _pose.translation()[0],
-          _pose.translation()[1],
-          _pose.translation()[2]));
+        convertVec(_pose.translation()));
   }
 }
 }  // namespace bullet_featherstone

bullet-featherstone/src/LinkFeatures.cc

@@ -59,7 +59,7 @@ void LinkFeatures::AddLinkExternalTorqueInWorld(
   auto *link = this->ReferenceInterface<LinkInfo>(_id);
   auto *model = this->ReferenceInterface<ModelInfo>(link->model);
 
-  auto T = btVector3(_torque[0], _torque[1], _torque[2]);
+  const btVector3 T = convertVec(_torque);
 
   if (link->indexInModel.has_value())
   {