From b8824702f4e7b427fcd611460b982865d5a4935d Mon Sep 17 00:00:00 2001 From: Shameek Ganguly Date: Tue, 6 Jun 2023 12:02:46 -0700 Subject: [PATCH 1/4] Minor cleanup of dartsim Signed-off-by: Shameek Ganguly --- dartsim/src/Base.hh | 4 ++-- dartsim/src/CustomHeightmapShape.cc | 1 - dartsim/src/JointFeatures.cc | 8 ++++---- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 8446fd7c0..1635a4cc1 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -469,8 +469,8 @@ class Base : public Implements3d> { // 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; } diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 0ae3e370a..672630b1e 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -19,7 +19,6 @@ #include #include -#include #include #include diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 5eda768e2..cd3161e48 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -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; } From 372c6fb22a3cddc1050f25c138856d14759e4a89 Mon Sep 17 00:00:00 2001 From: Shameek Ganguly Date: Wed, 7 Jun 2023 12:09:33 -0700 Subject: [PATCH 2/4] Clangtidy fixes Signed-off-by: Shameek Ganguly --- dartsim/src/Base.hh | 2 +- dartsim/src/Base_TEST.cc | 5 ++- dartsim/src/CustomHeightmapShape.hh | 2 +- dartsim/src/EntityManagementFeatures.cc | 7 ++-- dartsim/src/EntityManagementFeatures.hh | 6 +-- dartsim/src/FreeGroupFeatures.cc | 4 +- dartsim/src/JointFeatures.cc | 32 +++++++-------- dartsim/src/JointFeatures.hh | 52 ++++++++++++------------- dartsim/src/SDFFeatures.hh | 4 +- dartsim/src/ShapeFeatures.hh | 2 +- 10 files changed, 58 insertions(+), 58 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 1635a4cc1..684d570c1 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -161,7 +161,7 @@ struct EntityStorage return idToObject.at(_id); } - const std::optional MaybeAt(const std::size_t _id) const + std::optional MaybeAt(const std::size_t _id) const { auto it = this->idToObject.find(_id); if (it != this->idToObject.end()) diff --git a/dartsim/src/Base_TEST.cc b/dartsim/src/Base_TEST.cc index a9f8d156a..1d8a37352 100644 --- a/dartsim/src/Base_TEST.cc +++ b/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 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); diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index 3358df8af..6461a80a8 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -39,7 +39,7 @@ class CustomHeightmapShape : public dart::dynamics::HeightmapShape public: CustomHeightmapShape( const common::HeightmapData &_input, const Eigen::Vector3d &_size, - const int _subSampling); + int _subSampling); }; } } diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 17f3b8fd0..48ae13e07 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/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 GetFilterPtr( +static std::shared_ptr 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(_shapeID)->node; const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); diff --git a/dartsim/src/EntityManagementFeatures.hh b/dartsim/src/EntityManagementFeatures.hh index 82e5fe4dd..10d6bb41f 100644 --- a/dartsim/src/EntityManagementFeatures.hh +++ b/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; diff --git a/dartsim/src/FreeGroupFeatures.cc b/dartsim/src/FreeGroupFeatures.cc index bdbd6a57b..70b8c04fa 100644 --- a/dartsim/src/FreeGroupFeatures.cc +++ b/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))) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index cd3161e48..14c5fc7a4 100644 --- a/dartsim/src/JointFeatures.cc +++ b/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(_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(_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(_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(_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(_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(_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(_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(_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(_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(_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(_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(_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(_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(_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(_id)->joint; @@ -505,7 +505,7 @@ Identity JointFeatures::AttachFixedJoint( const std::size_t jointID = this->AddJoint( bn->moveTo(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; diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 62201d765..709d7dee8 100644 --- a/dartsim/src/JointFeatures.hh +++ b/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( diff --git a/dartsim/src/SDFFeatures.hh b/dartsim/src/SDFFeatures.hh index 901a4bb96..11c9bab33 100644 --- a/dartsim/src/SDFFeatures.hh +++ b/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, diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index 0c4d5370e..d1ff78cce 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -148,7 +148,7 @@ class ShapeFeatures : public: Identity AttachEllipsoidShape( const Identity &_linkID, const std::string &_name, - const Vector3d _radii, + Vector3d _radii, const Pose3d &_pose) override; // ----- Sphere Features ----- From b80c0d841b4375f26e5ecbe702675843e71ee893 Mon Sep 17 00:00:00 2001 From: Shameek Ganguly Date: Wed, 7 Jun 2023 14:17:18 -0700 Subject: [PATCH 3/4] Change radii to const reference Signed-off-by: Shameek Ganguly --- dartsim/src/ShapeFeatures.cc | 2 +- dartsim/src/ShapeFeatures.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index dfbf01277..8b42a0dbb 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -252,7 +252,7 @@ Vector3d ShapeFeatures::GetEllipsoidShapeRadii( Identity ShapeFeatures::AttachEllipsoidShape( const Identity &_linkID, const std::string &_name, - const Vector3d _radii, + const Vector3d &_radii, const Pose3d &_pose) { common::MeshManager *meshMgr = common::MeshManager::Instance(); diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index d1ff78cce..e83992419 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -148,7 +148,7 @@ class ShapeFeatures : public: Identity AttachEllipsoidShape( const Identity &_linkID, const std::string &_name, - Vector3d _radii, + const Vector3d &_radii, const Pose3d &_pose) override; // ----- Sphere Features ----- From 39e475315769ed20a77e5c2e8ff1e3fb32795f30 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 7 Aug 2023 13:18:31 -0700 Subject: [PATCH 4/4] Revert changes that change public ABI Continue passing Vector3 by value to AttachEllipsoidShape on garden. This has already been changed to pass by const ref on main. Signed-off-by: Steve Peters --- dartsim/src/ShapeFeatures.cc | 2 +- dartsim/src/ShapeFeatures.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 8b42a0dbb..dfbf01277 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -252,7 +252,7 @@ Vector3d ShapeFeatures::GetEllipsoidShapeRadii( Identity ShapeFeatures::AttachEllipsoidShape( const Identity &_linkID, const std::string &_name, - const Vector3d &_radii, + const Vector3d _radii, const Pose3d &_pose) { common::MeshManager *meshMgr = common::MeshManager::Instance(); diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index e83992419..0c4d5370e 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -148,7 +148,7 @@ class ShapeFeatures : public: Identity AttachEllipsoidShape( const Identity &_linkID, const std::string &_name, - const Vector3d &_radii, + const Vector3d _radii, const Pose3d &_pose) override; // ----- Sphere Features -----