diff --git a/CMakeLists.txt b/CMakeLists.txt index d79763a20..c66deab1d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,7 +84,7 @@ gz_find_package(DART # Find bullet for the bullet plugin wrapper gz_find_package(GzBullet VERSION 2.87 - REQUIRED_BY bullet + REQUIRED_BY bullet bullet-featherstone PKGCONFIG bullet PKGCONFIG_VER_COMPARISON >=) diff --git a/bullet-featherstone/CMakeLists.txt b/bullet-featherstone/CMakeLists.txt index c4b135e91..d428dd3d6 100644 --- a/bullet-featherstone/CMakeLists.txt +++ b/bullet-featherstone/CMakeLists.txt @@ -5,7 +5,7 @@ gz_add_component(bullet-featherstone INTERFACE GET_TARGET_NAME features) link_directories(${BULLET_LIBRARY_DIRS}) -target_link_libraries(${features} INTERFACE IgnBullet::IgnBullet) +target_link_libraries(${features} INTERFACE GzBullet::GzBullet) gz_get_libsources_and_unittests(sources test_sources) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 5e196f7ba..3761996a0 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -37,8 +37,6 @@ WorldInfo::WorldInfo(std::string name_) dispatcher.get(), broadphase.get(), solver.get(), collisionConfiguration.get()); - /* TO-DO(Lobotuerk): figure out what this line does*/ - world->getSolverInfo().m_globalCfm = 0; btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher.get()); } diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc index 7cedca392..db571902e 100644 --- a/bullet-featherstone/src/KinematicsFeatures.cc +++ b/bullet-featherstone/src/KinematicsFeatures.cc @@ -50,18 +50,22 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( } else { - // If the Frame was not a Link then we can assume it's a Model because Free - // Groups are always Models underneath. If we ever support frame semantics - // for more than links, models, and free groups, then this implementation - // needs to change. - model = this->FrameInterface(_id); + // If the Frame was not a Link then check if it's a collision object + const auto collisionIt = this->collisions.find(_id.ID()); + if (collisionIt != this->collisions.end()) + { + throw std::runtime_error("Okay it happened"); + } } FrameData data; - data.pose = convert(model->body->getBaseWorldTransform()) - * model->baseInertiaToLinkFrame; - data.linearVelocity = convert(model->body->getBaseVel()); - data.linearAcceleration = convert(model->body->getBaseOmega()); + if(model && model->body) + { + data.pose = convert(model->body->getBaseWorldTransform()) + * model->baseInertiaToLinkFrame; + data.linearVelocity = convert(model->body->getBaseVel()); + data.linearAcceleration = convert(model->body->getBaseOmega()); + } return data; } diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 4fa01e7c4..4d9db2565 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -32,6 +32,7 @@ #include #include +#include #include #include @@ -149,8 +150,8 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) default: gzerr << "Joint type [" << (std::size_t)(joint->Type()) << "] is not supported by " - << "gz-physics-bullet-featherstone-plugin\n"; - return false; + << "gz-physics-bullet-featherstone-plugin." + << "Replaced by a fixed joint.\n"; } if (child == parent) @@ -207,7 +208,6 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) gzerr << "The Link [" << childLinkName << "] in Model [" << rootModelName << "] has multiple parent joints. That is not " << "supported by the gz-physics-bullet-featherstone plugin.\n"; - return false; } } @@ -276,6 +276,10 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) { // Every element in flatLinks should have a parent if the earlier validation // was done correctly. + if (parentOf.size() == 0) + { + break; + } const auto *parentJoint = parentOf.at(flatLinks[i]).joint; const auto *parentLink = @@ -379,10 +383,13 @@ Identity SDFFeatures::ConstructSdfModel( const Eigen::Isometry3d linkToComTf = gz::math::eigen3::convert( link->Inertial().Pose()); - const auto linkID = this->AddLink( - LinkInfo{link->Name(), i, modelID, linkToComTf.inverse()}); + if (linkIDs.find(link) == linkIDs.end()) + { + const auto linkID = this->AddLink( + LinkInfo{link->Name(), i, modelID, linkToComTf.inverse()}); + linkIDs.insert(std::make_pair(link, linkID)); + } - linkIDs.insert(std::make_pair(link, linkID)); const auto &M = link->Inertial().MassMatrix(); const double mass = M.Mass(); const auto inertia = btVector3(M.Ixx(), M.Iyy(), M.Izz()); @@ -398,134 +405,144 @@ Identity SDFFeatures::ConstructSdfModel( } } - const auto &parentInfo = structure.parentOf.at(link); - const auto *joint = parentInfo.joint; - const auto &parentLinkID = linkIDs.at( - parentInfo.model->LinkByName(joint->ParentName())); - const auto *parentLinkInfo = this->ReferenceInterface( - parentLinkID); - - int parentIndex = -1; - if (parentLinkInfo->indexInModel.has_value()) - parentIndex = *parentLinkInfo->indexInModel; - - Eigen::Isometry3d poseParentLinkToJoint; - Eigen::Isometry3d poseParentComToJoint; + if (structure.parentOf.size()) { - gz::math::Pose3d gzPoseParentToJoint; - const auto errors = joint->SemanticPose().Resolve( - gzPoseParentToJoint, joint->ParentName()); - if (!errors.empty()) + const auto &parentInfo = structure.parentOf.at(link); + const auto *joint = parentInfo.joint; + const auto &parentLinkID = linkIDs.at( + parentInfo.model->LinkByName(joint->ParentName())); + const auto *parentLinkInfo = this->ReferenceInterface( + parentLinkID); + + int parentIndex = -1; + if (parentLinkInfo->indexInModel.has_value()) + parentIndex = *parentLinkInfo->indexInModel; + + Eigen::Isometry3d poseParentLinkToJoint; + Eigen::Isometry3d poseParentComToJoint; { - gzerr << "An error occurred while resolving the transform of Joint [" - << joint->Name() << "] in Model [" << model->name << "]:\n"; - for (const auto &error : errors) + gz::math::Pose3d gzPoseParentToJoint; + const auto errors = joint->SemanticPose().Resolve( + gzPoseParentToJoint, joint->ParentName()); + if (!errors.empty()) { - gzerr << error << "\n"; + gzerr << "An error occurred while resolving the transform of Joint [" + << joint->Name() << "] in Model [" << model->name << "]:\n"; + for (const auto &error : errors) + { + gzerr << error << "\n"; + } + + return this->GenerateInvalidId(); } - return this->GenerateInvalidId(); + poseParentLinkToJoint = gz::math::eigen3::convert(gzPoseParentToJoint); + poseParentComToJoint = + poseParentLinkToJoint * parentLinkInfo->inertiaToLinkFrame; } - poseParentLinkToJoint = gz::math::eigen3::convert(gzPoseParentToJoint); - poseParentComToJoint = - poseParentLinkToJoint * parentLinkInfo->inertiaToLinkFrame; - } - - Eigen::Isometry3d poseJointToChild; - { - gz::math::Pose3d gzPoseJointToChild; - const auto errors = - link->SemanticPose().Resolve(gzPoseJointToChild, joint->Name()); - if (!errors.empty()) + Eigen::Isometry3d poseJointToChild; { - gzerr << "An error occured while resolving the transform of Link [" - << link->Name() << "]:\n"; - for (const auto &error : errors) + gz::math::Pose3d gzPoseJointToChild; + const auto errors = + link->SemanticPose().Resolve(gzPoseJointToChild, joint->Name()); + if (!errors.empty()) { - gzerr << error << "\n"; + gzerr << "An error occured while resolving the transform of Link [" + << link->Name() << "]:\n"; + for (const auto &error : errors) + { + gzerr << error << "\n"; + } + + return this->GenerateInvalidId(); } - return this->GenerateInvalidId(); + poseJointToChild = gz::math::eigen3::convert(gzPoseJointToChild); } - poseJointToChild = gz::math::eigen3::convert(gzPoseJointToChild); - } + btQuaternion btRotParentComToJoint; + convertMat(poseParentComToJoint.linear()) + .getRotation(btRotParentComToJoint); - btQuaternion btRotParentComToJoint; - convertMat(poseParentComToJoint.linear()) - .getRotation(btRotParentComToJoint); + btVector3 btPosParentComToJoint = + convertVec(poseParentComToJoint.translation()); - btVector3 btPosParentComToJoint = - convertVec(poseParentComToJoint.translation()); + btVector3 btJointToChildCom = + convertVec((poseJointToChild * linkToComTf).translation()); - btVector3 btJointToChildCom = - convertVec((poseJointToChild * linkToComTf).translation()); - - this->AddJoint( - JointInfo{ - joint->Name(), - InternalJoint{i}, - model->linkEntityIds[parentIndex+1], - linkID, - poseParentLinkToJoint, - poseJointToChild, - modelID - }); - - if (::sdf::JointType::FIXED == joint->Type()) - { - model->body->setupFixed( - i, mass, inertia, parentIndex, - btRotParentComToJoint, - btPosParentComToJoint, - btJointToChildCom); - } - else if (::sdf::JointType::REVOLUTE == joint->Type()) - { - const auto axis = joint->Axis()->Xyz(); - model->body->setupRevolute( - i, mass, inertia, parentIndex, - btRotParentComToJoint, - btVector3(axis[0], axis[1], axis[2]), - btPosParentComToJoint, - btJointToChildCom, - true); - } - else if (::sdf::JointType::PRISMATIC == joint->Type()) - { - const auto axis = joint->Axis()->Xyz(); - model->body->setupPrismatic( - i, mass, inertia, parentIndex, - btRotParentComToJoint, - btVector3(axis[0], axis[1], axis[2]), - btPosParentComToJoint, - btJointToChildCom, - true); - } - else if (::sdf::JointType::BALL == joint->Type()) - { - model->body->setupSpherical( - i, mass, inertia, parentIndex, - btRotParentComToJoint, - btPosParentComToJoint, - btJointToChildCom); - } - else - { - gzerr << "You have found a bug in gz-physics-bullet-featherstone. Joint " - << "type [" << (std::size_t)(joint->Type()) << "] should have been " - << "filtered out during model validation, but that filtering " - << "failed. Please report this to the gz-physics developers\n"; - return this->GenerateInvalidId(); - } + this->AddJoint( + JointInfo{ + joint->Name(), + InternalJoint{i}, + model->linkEntityIds[parentIndex+1], + linkIDs.find(link)->second, + poseParentLinkToJoint, + poseJointToChild, + modelID + }); - for (std::size_t c = 0; c < link->CollisionCount(); ++c) - { - // If we fail to add the collision, just keep building the model. It may - // need to be constructed outside of the SDF generation pipeline, e.g. - // with AttachHeightmap. - this->AddSdfCollision(linkID, *link->CollisionByIndex(c)); + if (::sdf::JointType::FIXED == joint->Type()) + { + model->body->setupFixed( + i, mass, inertia, parentIndex, + btRotParentComToJoint, + btPosParentComToJoint, + btJointToChildCom); + } + else if (::sdf::JointType::REVOLUTE == joint->Type()) + { + const auto axis = joint->Axis()->Xyz(); + model->body->setupRevolute( + i, mass, inertia, parentIndex, + btRotParentComToJoint, + btVector3(axis[0], axis[1], axis[2]), + btPosParentComToJoint, + btJointToChildCom, + true); + } + else if (::sdf::JointType::PRISMATIC == joint->Type()) + { + const auto axis = joint->Axis()->Xyz(); + model->body->setupPrismatic( + i, mass, inertia, parentIndex, + btRotParentComToJoint, + btVector3(axis[0], axis[1], axis[2]), + btPosParentComToJoint, + btJointToChildCom, + true); + } + else if (::sdf::JointType::BALL == joint->Type()) + { + model->body->setupSpherical( + i, mass, inertia, parentIndex, + btRotParentComToJoint, + btPosParentComToJoint, + btJointToChildCom); + } + else + { + model->body->setupFixed( + i, mass, inertia, parentIndex, + btRotParentComToJoint, + btPosParentComToJoint, + btJointToChildCom); + } + if (::sdf::JointType::PRISMATIC == joint->Type() || + ::sdf::JointType::REVOLUTE == joint->Type()) + { + model->body->getLink(i).m_jointLowerLimit = joint->Axis()->Lower(); + model->body->getLink(i).m_jointUpperLimit = joint->Axis()->Upper(); + model->body->getLink(i).m_jointDamping = joint->Axis()->Damping(); + model->body->getLink(i).m_jointFriction = joint->Axis()->Friction(); + model->body->getLink(i).m_jointMaxVelocity = + joint->Axis()->MaxVelocity(); + model->body->getLink(i).m_jointMaxForce = joint->Axis()->Effort(); + + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint( + model->body.get(), i, joint->Axis()->Lower(), joint->Axis()->Upper()); + world->world->addMultiBodyConstraint(con); + } } } @@ -533,7 +550,6 @@ Identity SDFFeatures::ConstructSdfModel( model->body->finalizeMultiDof(); - const auto worldToModel = ResolveSdfPose(_sdfModel.SemanticPose()); if (!worldToModel) return this->GenerateInvalidId(); @@ -551,13 +567,26 @@ Identity SDFFeatures::ConstructSdfModel( model->body->setBaseOmega(btVector3(0, 0, 0)); world->world->addMultiBody(model->body.get()); + + for (const auto& [linkSdf, linkID] : linkIDs) + { + for (std::size_t c = 0; c < linkSdf->CollisionCount(); ++c) + { + // If we fail to add the collision, just keep building the model. It may + // need to be constructed outside of the SDF generation pipeline, e.g. + // with AttachHeightmap. + this->AddSdfCollision(linkID, *linkSdf->CollisionByIndex(c), isStatic); + } + } + return modelID; } ///////////////////////////////////////////////// bool SDFFeatures::AddSdfCollision( const Identity &_linkID, - const ::sdf::Collision &_collision) + const ::sdf::Collision &_collision, + bool isStatic) { if (!_collision.Geom()) { @@ -626,6 +655,7 @@ bool SDFFeatures::AddSdfCollision( double mu = 1.0; double mu2 = 1.0; double restitution = 0.0; + double rollingFriction = 0.0; if (const auto *surface = _collision.Surface()) { @@ -664,6 +694,7 @@ bool SDFFeatures::AddSdfCollision( } } + Eigen::Isometry3d linkFrameToCollision; if (shape != nullptr) { int linkIndexInModel = -1; @@ -678,8 +709,33 @@ bool SDFFeatures::AddSdfCollision( // for different shapes attached to the same link. auto collider = std::make_unique( model->body.get(), linkIndexInModel); - collider->setCollisionShape(linkInfo->shape.get()); + { + gz::math::Pose3d gzLinkToCollision; + const auto errors = + _collision.SemanticPose().Resolve(gzLinkToCollision, linkInfo->name); + if (!errors.empty()) + { + gzerr << "An error occurred while resolving the transform of the " + << "collider [" << _collision.Name() << "] in Link [" + << linkInfo->name << "] in Model [" << model->name << "]:\n"; + for (const auto &error : errors) + { + gzerr << error << "\n"; + } + + return false; + } + + linkFrameToCollision = gz::math::eigen3::convert(gzLinkToCollision); + } + + const btTransform btInertialToCollision = + convertTf(linkInfo->inertiaToLinkFrame * linkFrameToCollision); + + linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); + + collider->setCollisionShape(linkInfo->shape.get()); collider->setRestitution(restitution); collider->setRollingFriction(rollingFriction); collider->setFriction(mu); @@ -693,14 +749,35 @@ bool SDFFeatures::AddSdfCollision( { model->body->getLink(linkIndexInModel).m_collider = linkInfo->collider.get(); + const auto p = model->body->localPosToWorld( + linkIndexInModel, btVector3(0, 0, 0)); + const auto rot = model->body->localFrameToWorld( + linkIndexInModel, btMatrix3x3::getIdentity()); + linkInfo->collider->setWorldTransform(btTransform(rot, p)); } else { model->body->setBaseCollider(linkInfo->collider.get()); + linkInfo->collider->setWorldTransform( + model->body->getBaseWorldTransform()); } auto *world = this->ReferenceInterface(model->world); - world->world->addCollisionObject(linkInfo->collider.get()); + + if (isStatic) + { + world->world->addCollisionObject( + linkInfo->collider.get(), + btBroadphaseProxy::StaticFilter, + btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + } + else + { + world->world->addCollisionObject( + linkInfo->collider.get(), + btBroadphaseProxy::DefaultFilter, + btBroadphaseProxy::AllFilter); + } } else { @@ -708,31 +785,6 @@ bool SDFFeatures::AddSdfCollision( // match the existing collider and issue a warning if they don't. } - Eigen::Isometry3d linkFrameToCollision; - { - gz::math::Pose3d gzLinkToCollision; - const auto errors = - _collision.SemanticPose().Resolve(gzLinkToCollision, linkInfo->name); - if (!errors.empty()) - { - gzerr << "An error occurred while resolving the transform of the " - << "collider [" << _collision.Name() << "] in Link [" - << linkInfo->name << "] in Model [" << model->name << "]:\n"; - for (const auto &error : errors) - { - gzerr << error << "\n"; - } - - return false; - } - - linkFrameToCollision = gz::math::eigen3::convert(gzLinkToCollision); - } - - const btTransform btInertialToCollision = - convertTf(linkInfo->inertiaToLinkFrame * linkFrameToCollision); - - linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); this->AddCollision( CollisionInfo{ _collision.Name(), diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index da699a8d9..67d5859e2 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/bullet-featherstone/src/SDFFeatures.hh @@ -51,7 +51,8 @@ class SDFFeatures : public: bool AddSdfCollision( const Identity &_linkID, - const ::sdf::Collision &_collision); + const ::sdf::Collision &_collision, + bool isStatic); }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc new file mode 100644 index 000000000..16e93b902 --- /dev/null +++ b/bullet-featherstone/src/ShapeFeatures.cc @@ -0,0 +1,381 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "ShapeFeatures.hh" +#include + +#include +#include +#include + +namespace gz { +namespace physics { +namespace bullet_featherstone { + +///////////////////////////////////////////////// +AlignedBox3d ShapeFeatures::GetShapeAxisAlignedBoundingBox( + const Identity &_shapeID) const +{ + const auto *collider = this->ReferenceInterface(_shapeID); + if (collider) + { + btTransform t; + t.setIdentity(); + btVector3 minBox(0, 0, 0); + btVector3 maxBox(0, 0, 0); + btVector3 minBox2(0, 0, 0); + btVector3 maxBox2(0, 0, 0); + collider->collider->getAabb(t, minBox, maxBox); + return math::eigen3::convert(math::AxisAlignedBox( + math::Vector3d(minBox[0], minBox[1], minBox[2]), + math::Vector3d(maxBox[0], maxBox[1], maxBox[2]))); + } + return math::eigen3::convert(math::AxisAlignedBox()); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToBoxShape( + const Identity &_shapeID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_shapeID); + if (shapeInfo != nullptr) + { + const auto &shape = shapeInfo->collider; + if (dynamic_cast(shape.get())) + return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); + } + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +LinearVector3d ShapeFeatures::GetBoxShapeSize( + const Identity &_boxID) const +{ + // _boxID ~= _collisionID + auto it = this->collisions.find(_boxID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *box = static_cast(it->second->collider.get()); + btVector3 v = box->getHalfExtentsWithMargin(); + return math::eigen3::convert(math::Vector3d(v[0], v[1], v[2]) * 2); + } + } + // return invalid box shape size if no collision found + return math::eigen3::convert(math::Vector3d(-1.0, -1.0, -1.0)); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachBoxShape( + const Identity &_linkID, + const std::string &_name, + const LinearVector3d &_size, + const Pose3d &_pose) +{ + const auto size = math::eigen3::convert(_size); + const btVector3 halfExtents = btVector3(size.X(), size.Y(), size.Z()) * 0.5; + std::unique_ptr shape = + std::make_unique(halfExtents); + + auto identity = this->AddCollision( + CollisionInfo{ + _name, + std::move(shape), + _linkID, + Eigen::Isometry3d()}); + + return identity; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToCapsuleShape(const Identity &_shapeID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_shapeID); + if (shapeInfo != nullptr) + { + const auto &shape = shapeInfo->collider; + if (dynamic_cast(shape.get())) + return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); + } + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetCapsuleShapeRadius( + const Identity &_capsuleID) const +{ + auto it = this->collisions.find(_capsuleID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *capsule = static_cast( + it->second->collider.get()); + if (capsule) + { + return capsule->getRadius(); + } + } + } + + return -1; +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetCapsuleShapeLength( + const Identity &_capsuleID) const +{ + auto it = this->collisions.find(_capsuleID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *capsule = static_cast( + it->second->collider.get()); + if (capsule) + { + return capsule->getHalfHeight() * 2; + } + } + } + + return -1; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachCapsuleShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const double _length, + const Pose3d &_pose) +{ + auto shape = + std::make_unique(_radius, _length / 2); + + auto identity = this->AddCollision( + CollisionInfo{ + _name, + std::move(shape), + _linkID, + Eigen::Isometry3d()}); + + return identity; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_shapeID); + if (shapeInfo != nullptr) + { + const auto &shape = shapeInfo->collider; + if (dynamic_cast(shape.get())) + return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); + } + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetCylinderShapeRadius( + const Identity &_cylinderID) const +{ + auto it = this->collisions.find(_cylinderID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *cylinder = static_cast( + it->second->collider.get()); + if (cylinder) + { + return cylinder->getHalfExtentsWithMargin()[0]; + } + } + } + + return -1; +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetCylinderShapeHeight( + const Identity &_cylinderID) const +{ + auto it = this->collisions.find(_cylinderID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *cylinder = static_cast( + it->second->collider.get()); + if (cylinder) + { + return cylinder->getHalfExtentsWithMargin()[2] * 2; + } + } + } + + return -1; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachCylinderShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const double _height, + const Pose3d &_pose) +{ + const auto radius = _radius; + const auto halfLength = _height * 0.5; + auto shape = + std::make_unique(btVector3(radius, radius, halfLength)); + + auto identity = this->AddCollision( + CollisionInfo{ + _name, + std::move(shape), + _linkID, + Eigen::Isometry3d()}); + + return identity; +} + + +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToEllipsoidShape(const Identity &_shapeID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_shapeID); + if (shapeInfo != nullptr) + { + const auto &shape = shapeInfo->collider; + if (dynamic_cast(shape.get())) + return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); + } + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +Vector3d ShapeFeatures::GetEllipsoidShapeRadii( + const Identity &_ellipsoidID) const +{ + auto it = this->collisions.find(_ellipsoidID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *ellipsoid = static_cast( + it->second->collider.get()); + if (ellipsoid) + { + auto radii = ellipsoid->getLocalScaling(); + return Vector3d(radii[0], radii[1], radii[2]); + } + } + } + + return Vector3d(-1, -1, -1); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachEllipsoidShape( + const Identity &_linkID, + const std::string &_name, + const Vector3d _radii, + const Pose3d &_pose) +{ + btVector3 positions[1]; + btScalar radius[1]; + positions[0] = btVector3(); + radius[0] = 1; + + auto btSphere = std::make_unique( + positions, radius, 1); + btSphere->setLocalScaling(btVector3(_radii[0], _radii[1], _radii[2])); + auto shape = std::move(btSphere); + + auto identity = this->AddCollision( + CollisionInfo{ + _name, + std::move(shape), + _linkID, + Eigen::Isometry3d()}); + return identity; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToSphereShape( + const Identity &_shapeID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_shapeID); + if (shapeInfo != nullptr) + { + const auto &shape = shapeInfo->collider; + if (dynamic_cast(shape.get())) + return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); + } + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetSphereShapeRadius(const Identity &_sphereID) const +{ + auto it = this->collisions.find(_sphereID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *sphere = static_cast(it->second->collider.get()); + if (sphere) + { + return sphere->getRadius(); + } + } + } + + return -1; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachSphereShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const Pose3d &_pose) +{ + std::unique_ptr shape = + std::make_unique(_radius); + + auto identity = this->AddCollision( + CollisionInfo{ + _name, + std::move(shape), + _linkID, + Eigen::Isometry3d()}); + + return identity; +} + +} +} +} diff --git a/bullet-featherstone/src/ShapeFeatures.hh b/bullet-featherstone/src/ShapeFeatures.hh new file mode 100644 index 000000000..143f892fb --- /dev/null +++ b/bullet-featherstone/src/ShapeFeatures.hh @@ -0,0 +1,141 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_SHAPEFEATURES_HH_ +#define GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_SHAPEFEATURES_HH_ + +#include +#include +#include +#include +#include +#include + +#include + +#include "Base.hh" + +namespace gz { +namespace physics { +namespace bullet_featherstone { + +struct ShapeFeatureList : FeatureList< + GetShapeBoundingBox, + + GetBoxShapeProperties, + AttachBoxShapeFeature, + + GetCapsuleShapeProperties, + AttachCapsuleShapeFeature, + + GetCylinderShapeProperties, + AttachCylinderShapeFeature, + + GetEllipsoidShapeProperties, + AttachEllipsoidShapeFeature, + + GetSphereShapeProperties, + AttachSphereShapeFeature +> { }; + +class ShapeFeatures : + public virtual Base, + public virtual Implements3d +{ + // ----- Boundingbox Features ----- + public: AlignedBox3d GetShapeAxisAlignedBoundingBox( + const Identity &_shapeID) const override; + + // ----- Box Features ----- + public: Identity CastToBoxShape( + const Identity &_shapeID) const override; + + public: LinearVector3d GetBoxShapeSize( + const Identity &_boxID) const override; + + public: Identity AttachBoxShape( + const Identity &_linkID, + const std::string &_name, + const LinearVector3d &_size, + const Pose3d &_pose) override; + + // ----- Capsule Features ----- + public: Identity CastToCapsuleShape( + const Identity &_shapeID) const override; + + public: double GetCapsuleShapeRadius( + const Identity &_capsuleID) const override; + + public: double GetCapsuleShapeLength( + const Identity &_capsuleID) const override; + + public: Identity AttachCapsuleShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _length, + const Pose3d &_pose) override; + + // ----- Cylinder Features ----- + public: Identity CastToCylinderShape( + const Identity &_shapeID) const override; + + public: double GetCylinderShapeRadius( + const Identity &_cylinderID) const override; + + public: double GetCylinderShapeHeight( + const Identity &_cylinderID) const override; + + public: Identity AttachCylinderShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _height, + const Pose3d &_pose) override; + + // ----- Ellipsoid Features ----- + public: Identity CastToEllipsoidShape( + const Identity &_shapeID) const override; + + public: Vector3d GetEllipsoidShapeRadii( + const Identity &_ellipsoidID) const override; + + public: Identity AttachEllipsoidShape( + const Identity &_linkID, + const std::string &_name, + const Vector3d _radii, + const Pose3d &_pose) override; + + // ----- Sphere Features ----- + public: Identity CastToSphereShape( + const Identity &_shapeID) const override; + + public: double GetSphereShapeRadius( + const Identity &_sphereID) const override; + + public: Identity AttachSphereShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + const Pose3d &_pose) override; +}; + +} +} +} + +#endif diff --git a/bullet-featherstone/src/plugin.cc b/bullet-featherstone/src/plugin.cc index 17f089658..d46fbba56 100644 --- a/bullet-featherstone/src/plugin.cc +++ b/bullet-featherstone/src/plugin.cc @@ -23,6 +23,7 @@ #include "Base.hh" #include "EntityManagementFeatures.hh" #include "FreeGroupFeatures.hh" +#include "ShapeFeatures.hh" #include "JointFeatures.hh" #include "KinematicsFeatures.hh" #include "SDFFeatures.hh" @@ -39,6 +40,7 @@ struct BulletFeatures : FeatureList < FreeGroupFeatureList, KinematicsFeatureList, SDFFeatureList, + ShapeFeatureList, JointFeatureList, WorldFeatureList > { }; @@ -51,6 +53,7 @@ class Plugin : public virtual FreeGroupFeatures, public virtual KinematicsFeatures, public virtual SDFFeatures, + public virtual ShapeFeatures, public virtual JointFeatures, public virtual WorldFeatures {}; diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 20ac570f8..750de0912 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -201,7 +201,6 @@ TYPED_TEST(SimulationFeaturesTestBasic, StepWorld) } } - ///////////////////////////////////////////////// TYPED_TEST(SimulationFeaturesTestBasic, Falling) { @@ -237,11 +236,16 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeFeatures) gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); for (const auto &world : worlds) { + std::cerr << "world model count " << world->GetModelCount() << '\n'; // test ShapeFeatures auto sphere = world->GetModel("sphere"); + EXPECT_NE(nullptr, sphere); auto sphereLink = sphere->GetLink(0); + EXPECT_NE(nullptr, sphereLink); auto sphereCollision = sphereLink->GetShape(0); + EXPECT_NE(nullptr, sphereCollision); auto sphereShape = sphereCollision->CastToSphereShape(); + EXPECT_NE(nullptr, sphereShape); EXPECT_NEAR(1.0, sphereShape->GetRadius(), 1e-6); EXPECT_EQ(1u, sphereLink->GetShapeCount()); @@ -251,9 +255,13 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeFeatures) EXPECT_EQ(sphere2, sphereLink->GetShape(1)); auto box = world->GetModel("box"); - auto boxLink = box->GetLink(0); + EXPECT_NE(nullptr, box); + auto boxLink = box->GetLink("box_link"); + EXPECT_NE(nullptr, boxLink); auto boxCollision = boxLink->GetShape(0); + EXPECT_NE(nullptr, boxCollision); auto boxShape = boxCollision->CastToBoxShape(); + EXPECT_NE(nullptr, boxShape); EXPECT_EQ(gz::math::Vector3d(100, 100, 1), gz::math::eigen3::convert(boxShape->GetSize())); @@ -264,6 +272,8 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeFeatures) Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, boxLink->GetShapeCount()); EXPECT_EQ(box2, boxLink->GetShape(1)); + EXPECT_EQ(gz::math::Vector3d(1.2, 1.2, 1.2), + gz::math::eigen3::convert(boxLink->GetShape(1)->CastToBoxShape()->GetSize())); auto cylinder = world->GetModel("cylinder"); auto cylinderLink = cylinder->GetLink(0); @@ -276,6 +286,12 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeFeatures) "cylinder2", 3.0, 4.0, Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, cylinderLink->GetShapeCount()); EXPECT_EQ(cylinder2, cylinderLink->GetShape(1)); + EXPECT_NEAR(3.0, + cylinderLink->GetShape(1)->CastToCylinderShape()->GetRadius(), + 1e-6); + EXPECT_NEAR(4.0, + cylinderLink->GetShape(1)->CastToCylinderShape()->GetHeight(), + 1e-6); auto ellipsoid = world->GetModel("ellipsoid"); auto ellipsoidLink = ellipsoid->GetLink(0); @@ -316,6 +332,15 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeFeatures) auto capsuleAABB = capsuleCollision->GetAxisAlignedBoundingBox(*capsuleCollision); + std::cerr << "gz::math::eigen3::convert(sphereAABB).Min() " << gz::math::eigen3::convert(sphereAABB).Min() << '\n'; + std::cerr << "gz::math::eigen3::convert(sphereAABB).Max() " << gz::math::eigen3::convert(sphereAABB).Max() << '\n'; + + std::cerr << "gz::math::eigen3::convert(boxAABB).Min() " << gz::math::eigen3::convert(boxAABB).Min() << '\n'; + std::cerr << "gz::math::eigen3::convert(boxAABB).Max() " << gz::math::eigen3::convert(boxAABB).Max() << '\n'; + + std::cerr << "gz::math::eigen3::convert(ellipsoidAABB).Min() " << gz::math::eigen3::convert(ellipsoidAABB).Min() << '\n'; + std::cerr << "gz::math::eigen3::convert(ellipsoidAABB).Max() " << gz::math::eigen3::convert(ellipsoidAABB).Max() << '\n'; + EXPECT_TRUE(gz::math::Vector3d(-1, -1, -1).Equal( gz::math::eigen3::convert(sphereAABB).Min(), 0.1)); EXPECT_EQ(gz::math::Vector3d(1, 1, 1), @@ -343,6 +368,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeFeatures) auto cylinderModelAABB = cylinder->GetAxisAlignedBoundingBox(); auto ellipsoidModelAABB = ellipsoid->GetAxisAlignedBoundingBox(); auto capsuleModelAABB = capsule->GetAxisAlignedBoundingBox(); + EXPECT_EQ(gz::math::Vector3d(-1, 0.5, -0.5), gz::math::eigen3::convert(sphereModelAABB).Min()); EXPECT_EQ(gz::math::Vector3d(1, 2.5, 1.5), @@ -952,6 +978,7 @@ TYPED_TEST(SimulationFeaturesTestFeaturesContactPropertiesCallback, ContactPrope int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); + if (!SimulationFeaturesTest::init( argc, argv)) return -1;