Merge branch 'gz-physics6' into relocbin2

dartsim/src/Base.hh

@@ -161,7 +161,7 @@ struct EntityStorage
     return idToObject.at(_id);
   }
 
-  const std::optional<Value1> MaybeAt(const std::size_t _id) const
+  std::optional<Value1> MaybeAt(const std::size_t _id) const
   {
     auto it = this->idToObject.find(_id);
     if (it != this->idToObject.end())
@@ -469,8 +469,8 @@ class Base : public Implements3d<FeatureList<Feature>>
     {
       // We have not found a welded node associated with _link. This shouldn't
       // happen.
-      ignerr << "Could not find welded body node for link " << _link->name
-             << ". Merging of link and welded body failed.";
+      gzerr << "Could not find welded body node for link " << _link->name
+            << ". Merging of link and welded body failed.";
       return;
     }
 

dartsim/src/Base_TEST.cc

@@ -134,14 +134,15 @@ TEST(BaseClass, RemoveModel)
   // Collect the model names in a vector so we can remove them from the modelIDs
   // map in a for loop.
   std::vector<std::string> modelNames;
-  for (auto item : modelIDs)
+  modelNames.reserve(modelIDs.size());
+  for (const auto& item : modelIDs)
   {
     modelNames.push_back(item.first);
   }
 
   // Remove all of the models while checking sizes and indices;
   std::size_t curSize = 4;
-  for (auto name : modelNames) {
+  for (const auto& name : modelNames) {
     auto modelID = modelIDs[name];
     base.RemoveModelImpl(worldID, modelID);
     modelIDs.erase(name);

dartsim/src/CustomHeightmapShape.cc

@@ -19,7 +19,6 @@
 
 #include <vector>
 #include <gz/common/Console.hh>
-#include <gz/common/geospatial/Dem.hh>
 #include <gz/common/geospatial/ImageHeightmap.hh>
 #include <gz/math/eigen3/Conversions.hh>
 

dartsim/src/CustomHeightmapShape.hh

@@ -39,7 +39,7 @@ class CustomHeightmapShape : public dart::dynamics::HeightmapShape<float>
   public: CustomHeightmapShape(
       const common::HeightmapData &_input,
       const Eigen::Vector3d &_size,
-      const int _subSampling);
+      int _subSampling);
 };
 }
 }

dartsim/src/EntityManagementFeatures.cc

@@ -64,8 +64,7 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter
     return false;
   }
 
-  public: void SetIgnoredCollision(DartShapeConstPtr _shapePtr,
-      const uint16_t _mask)
+  public: void SetIgnoredCollision(DartShapeConstPtr _shapePtr, uint16_t _mask)
   {
     bitmaskMap[_shapePtr] = _mask;
   }
@@ -103,7 +102,7 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter
 /// as RemoveModelByIndex, where we call GetFilterPtr followed by
 /// RemoveSkeletonCollisions(model). To de-duplicate this, move this logic into
 /// RemoveModelImpl. To do that, we need to move GetFilterPtr into Base.hh.
-static const std::shared_ptr<BitmaskContactFilter> GetFilterPtr(
+static std::shared_ptr<BitmaskContactFilter> GetFilterPtr(
     const EntityManagementFeatures* _emf, std::size_t _worldID)
 {
   const auto world = _emf->worlds.at(_worldID);
@@ -819,7 +818,7 @@ Identity EntityManagementFeatures::ConstructEmptyLink(
 }
 
 void EntityManagementFeatures::SetCollisionFilterMask(
-    const Identity &_shapeID, const uint16_t _mask)
+    const Identity &_shapeID, uint16_t _mask)
 {
   const auto shapeNode = this->ReferenceInterface<ShapeInfo>(_shapeID)->node;
   const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode);

dartsim/src/EntityManagementFeatures.hh

@@ -149,7 +149,7 @@ class EntityManagementFeatures :
   public: bool ModelRemoved(const Identity &_modelID) const override;
 
   public: bool RemoveNestedModelByIndex(
-     const Identity &_modelID, std::size_t _modelIndex) override;
+     const Identity &_modelID, std::size_t _nestedModelIndex) override;
 
   public: bool RemoveNestedModelByName(
       const Identity &_modelID, const std::string &_modelName) override;
@@ -162,14 +162,14 @@ class EntityManagementFeatures :
       const Identity &_worldID, const std::string &_name) override;
 
   public: Identity ConstructEmptyNestedModel(
-      const Identity &_modelID, const std::string &_name) override;
+      const Identity &_parentModelID, const std::string &_name) override;
 
   public: Identity ConstructEmptyLink(
       const Identity &_modelID, const std::string &_name) override;
 
   // ----- Manage collision filter masks -----
   public: void SetCollisionFilterMask(
-      const Identity &_shapeID, const uint16_t _mask) override;
+      const Identity &_shapeID, uint16_t _mask) override;
 
   public: uint16_t GetCollisionFilterMask(
       const Identity &_shapeID) const override;

dartsim/src/FreeGroupFeatures.cc

@@ -35,7 +35,7 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
 
   // If there are no bodies at all in this model, then the FreeGroup functions
   // will not work properly, so we'll just reject these cases.
-  if (skeleton->getNumBodyNodes() == 0 && modelInfo->nestedModels.size() == 0)
+  if (skeleton->getNumBodyNodes() == 0 && modelInfo->nestedModels.empty())
     return this->GenerateInvalidId();
 
   // Verify that all root joints are FreeJoints
@@ -58,7 +58,7 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
     // returning an error.
     auto nestedModelInfo = this->models.at(nestedModel);
     if (nestedModelInfo->model->getNumBodyNodes() > 0 ||
-        nestedModelInfo->nestedModels.size() > 0)
+        !nestedModelInfo->nestedModels.empty())
     {
       if (!this->FindFreeGroupForModel(
               this->GenerateIdentity(nestedModel, nestedModelInfo)))

dartsim/src/JointFeatures.cc

@@ -30,28 +30,28 @@ namespace dartsim {
 
 /////////////////////////////////////////////////
 double JointFeatures::GetJointPosition(
-    const Identity &_id, const std::size_t _dof) const
+    const Identity &_id, std::size_t _dof) const
 {
   return this->ReferenceInterface<JointInfo>(_id)->joint->getPosition(_dof);
 }
 
 /////////////////////////////////////////////////
 double JointFeatures::GetJointVelocity(
-    const Identity &_id, const std::size_t _dof) const
+    const Identity &_id, std::size_t _dof) const
 {
   return this->ReferenceInterface<JointInfo>(_id)->joint->getVelocity(_dof);
 }
 
 /////////////////////////////////////////////////
 double JointFeatures::GetJointAcceleration(
-    const Identity &_id, const std::size_t _dof) const
+    const Identity &_id, std::size_t _dof) const
 {
   return this->ReferenceInterface<JointInfo>(_id)->joint->getAcceleration(_dof);
 }
 
 /////////////////////////////////////////////////
 double JointFeatures::GetJointForce(
-    const Identity &_id, const std::size_t _dof) const
+    const Identity &_id, std::size_t _dof) const
 {
   return this->ReferenceInterface<JointInfo>(_id)->joint->getForce(_dof);
 }
@@ -65,7 +65,7 @@ Pose3d JointFeatures::GetJointTransform(const Identity &_id) const
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointPosition(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -84,7 +84,7 @@ void JointFeatures::SetJointPosition(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointVelocity(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -103,7 +103,7 @@ void JointFeatures::SetJointVelocity(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointAcceleration(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -122,7 +122,7 @@ void JointFeatures::SetJointAcceleration(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointForce(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -145,7 +145,7 @@ void JointFeatures::SetJointForce(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointVelocityCommand(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -186,7 +186,7 @@ void JointFeatures::SetJointVelocityCommand(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointMinPosition(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -211,7 +211,7 @@ void JointFeatures::SetJointMinPosition(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointMaxPosition(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -236,7 +236,7 @@ void JointFeatures::SetJointMaxPosition(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointMinVelocity(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -261,7 +261,7 @@ void JointFeatures::SetJointMinVelocity(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointMaxVelocity(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -286,7 +286,7 @@ void JointFeatures::SetJointMaxVelocity(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointMinEffort(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -306,7 +306,7 @@ void JointFeatures::SetJointMinEffort(
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointMaxEffort(
-    const Identity &_id, const std::size_t _dof, const double _value)
+    const Identity &_id, std::size_t _dof, double _value)
 {
   auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
 
@@ -393,10 +393,10 @@ void JointFeatures::DetachJoint(const Identity &_jointId)
   }
   else
   {
-    ignerr << "Could not find LinkInfo for child link [" << child->getName()
-           << "] when detaching joint "
-           << "[" << joint->getName() << "]. Joint detaching failed."
-           << std::endl;
+    gzerr << "Could not find LinkInfo for child link [" << child->getName()
+          << "] when detaching joint "
+          << "[" << joint->getName() << "]. Joint detaching failed."
+          << std::endl;
     return;
   }
 
@@ -505,7 +505,7 @@ Identity JointFeatures::AttachFixedJoint(
   const std::size_t jointID = this->AddJoint(
       bn->moveTo<dart::dynamics::WeldJoint>(parentBn, properties),
       fullJointName, modelID);
-  if (linkInfo->weldedNodes.size() > 0)
+  if (!linkInfo->weldedNodes.empty())
   {
     // weld constraint needs to be updated after moving to new skeleton
     auto constraint = linkInfo->weldedNodes.back().second;

dartsim/src/JointFeatures.hh

@@ -65,36 +65,36 @@ class JointFeatures :
 {
   // ----- Get Basic Joint State -----
   public: double GetJointPosition(
-      const Identity &_id, const std::size_t _dof) const override;
+      const Identity &_id, std::size_t _dof) const override;
 
   public: double GetJointVelocity(
-      const Identity &_id, const std::size_t _dof) const override;
+      const Identity &_id, std::size_t _dof) const override;
 
   public: double GetJointAcceleration(
-      const Identity &_id, const std::size_t _dof) const override;
+      const Identity &_id, std::size_t _dof) const override;
 
   public: double GetJointForce(
-      const Identity &_id, const std::size_t _dof) const override;
+      const Identity &_id, std::size_t _dof) const override;
 
   public: Pose3d GetJointTransform(const Identity &_id) const override;
 
 
   // ----- Set Basic Joint State -----
   public: void SetJointPosition(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   public: void SetJointVelocity(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   public: void SetJointAcceleration(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   public: void SetJointForce(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
 
   // ----- Get Basic Joint Properties -----
@@ -173,32 +173,32 @@ class JointFeatures :
 
   // ----- Joint Commands -----
   public: void SetJointVelocityCommand(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   public: void SetJointMinPosition(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   public: void SetJointMaxPosition(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   public: void SetJointMinVelocity(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   public: void SetJointMaxVelocity(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   public: void SetJointMinEffort(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   public: void SetJointMaxEffort(
-      const Identity &_id, const std::size_t _dof,
-      const double _value) override;
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
 
   // ----- Transmitted wrench -----
   public: Wrench3d GetJointTransmittedWrenchInJointFrame(

dartsim/src/SDFFeatures.hh

@@ -97,8 +97,8 @@ class SDFFeatures :
   /// to be world
   private: Identity ConstructSdfJoint(const Identity &_modelID,
       const ::sdf::Joint &_sdfJoint,
-      dart::dynamics::BodyNode * const _parent,
-      dart::dynamics::BodyNode * const _child);
+      dart::dynamics::BodyNode * _parent,
+      dart::dynamics::BodyNode * _child);
 
   private: Eigen::Isometry3d ResolveSdfLinkReferenceFrame(
       const std::string &_frame,