From ca1715e2a41ecb070fe8d81d7530f75e0c985339 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 5 Mar 2024 01:34:07 +0000 Subject: [PATCH] typos, comments, style Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 6 +++--- .../src/EntityManagementFeatures.cc | 2 -- bullet-featherstone/src/SDFFeatures.cc | 18 ++++++++++-------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 231ddf0c0..4e9bb4338 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -394,8 +394,6 @@ class Base : public Implements3d> if (!model) return false; - bool isNested = this->worlds.find(_parentID) == this->worlds.end(); - // Remove nested models for (auto &nestedModelID : model->nestedModelEntityIds) { @@ -423,7 +421,9 @@ class Base : public Implements3d> } } - // If nested, we are done here. No need to remove multibody + // If nested, no need to remove multibody + // \todo(iche033) Remove links and joints in nested model + bool isNested = this->worlds.find(_parentID) == this->worlds.end(); if (isNested) { return true; diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 99e40b827..775c2948b 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -206,8 +206,6 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { auto *model = this->ReferenceInterface(_modelID); - if (!model) - return false; return this->RemoveModelImpl(model->world, _modelID); } diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 464c5cb6e..0bbdfc11e 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -179,7 +179,7 @@ ::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, return errors; } - // compute joint pose relative to parent link + // compute joint pose relative to specified link const auto *link = _model->LinkByName(_link); if (!link) { @@ -204,6 +204,7 @@ ::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, ///////////////////////////////////////////////// /// \brief Recursively build a tree of parent-child data structures from the +/// input Model SDF. /// \param[in] _sdfModel input Model SDF. /// \param[out] _parentOf A map of child link to its parent /// \param[out] _linkTree A map of parent link to its child links @@ -302,7 +303,8 @@ bool buildTrees(const ::sdf::Model *_sdfModel, /// \brief Find all the root links given a model SDF /// \param[in] _sdfModel Model SDF /// \param[in] _parentOf A map of child link to its parent info -/// \param[out] _rootLinks A vector of root links and its immediate parent model +/// \param[out] _rootLinks A vector of root links paired with their +/// immediate parent model void findRootLinks(const ::sdf::Model *_sdfModel, const std::unordered_map &_parentOf, std::vector> &_rootLinks) @@ -472,12 +474,13 @@ Identity SDFFeatures::ConstructSdfModelImpl( // multiple sub-trees detected in model // \todo(iche033) support multiple kinematic trees and // multiple floating links in a single model + // https://github.com/gazebosim/gz-physics/issues/550 gzerr << "Multiple sub-trees / floating links detected in a model. " << "This is not supported in bullet-featherstone implementation yet." << std::endl; } // Create model for the first structure. - auto structure = structures[0]; + auto &structure = structures[0]; const bool isStatic = _sdfModel.Static(); WorldInfo *world = nullptr; @@ -688,7 +691,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( btTransform parentLocalInertialFrame = convertTf( parentLinkInfo->inertiaToLinkFrame); btTransform parent2jointBt = convertTf( - // gz::math::eigen3::convert(parent2joint.Inverse())); // X_PJ poseParentLinkToJoint); // X_PJ // offsetInABt = parent COM to pivot (X_IpJ) @@ -702,7 +704,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( btTransform offsetInABt, offsetInBBt; offsetInABt = parentLocalInertialFrame * parent2jointBt; offsetInBBt = - convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); + convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); // R_IcJ * R_IpJ ^ -1 = R_IcIp; btQuaternion parentRotToThis = offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); @@ -802,7 +804,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( return this->GenerateInvalidId(); auto modelToNestedModel = Eigen::Isometry3d::Identity(); - // if the model of the root link is in a top level model, compute + // if the model of the root link is nested in a top level model, compute // its pose relative to top level model if (structure.model != &_sdfModel) { @@ -1267,7 +1269,7 @@ Identity SDFFeatures::ConstructSdfJoint( // >= 2.89. This is needed for dynamically creating world joints, // i.e. setting the btMultiBody to be fixed. So output an error letting // users know the joint will not be created. - // \todo(iche033) An workaround for this is to loop through all the joints + // \todo(iche033) A workaround for this is to loop through all the joints // in the world first in ConstructSdfWorld, keep track of the models who are // a child of the world joint, then when creating the btMultiBody // in ConstructSdfModelImpl, pass fixedBase as true in its constructor. @@ -1297,7 +1299,7 @@ Identity SDFFeatures::ConstructSdfJoint( childLinkName = _sdfJoint.ChildName(); } - // Currently only supports constructing joint with world + // Currently only supports constructing fixed joint with world as parent if (parentLinkName == "world" && _sdfJoint.Type() == ::sdf::JointType::FIXED) { auto worldModelIt = this->models.find(_modelID);