From cca7e923702b1711f4f16db8c869eec5e710acf7 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 9 Jul 2024 14:05:05 -0700 Subject: [PATCH] bullet-featherstone: Support empty links (#665) Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 169 ++++++++++++++----------- bullet-featherstone/src/SDFFeatures.hh | 8 ++ test/common_test/worlds/test.world | 16 --- 3 files changed, 101 insertions(+), 92 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index ffc95c846..ebb7c686c 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -890,12 +890,21 @@ Identity SDFFeatures::ConstructSdfModelImpl( for (const auto& [linkSdf, linkID] : linkIDs) { - for (std::size_t c = 0; c < linkSdf->CollisionCount(); ++c) + if (linkSdf->CollisionCount() == 0u) { - // 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); + // create empty link collider and compound shape if there are no + // collisions + this->CreateLinkCollider(linkID, isStatic); + } + else + { + 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); + } } } @@ -955,7 +964,7 @@ Identity SDFFeatures::ConstructSdfModel( bool SDFFeatures::AddSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision, - bool isStatic) + bool _isStatic) { if (!_collision.Geom()) { @@ -1283,22 +1292,11 @@ bool SDFFeatures::AddSdfCollision( const btTransform btInertialToCollision = convertTf(linkInfo->inertiaToLinkFrame * linkFrameToCollision); - int linkIndexInModel = -1; - if (linkInfo->indexInModel.has_value()) - linkIndexInModel = *linkInfo->indexInModel; - if (!linkInfo->collider) { - linkInfo->shape = std::make_unique(); - - // NOTE: Bullet does not appear to support different surface properties - // for different shapes attached to the same link. - linkInfo->collider = std::make_unique( - model->body.get(), linkIndexInModel); + this->CreateLinkCollider(_linkID, _isStatic, shape.get(), + btInertialToCollision); - linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); - - linkInfo->collider->setCollisionShape(linkInfo->shape.get()); linkInfo->collider->setRestitution(static_cast(restitution)); linkInfo->collider->setRollingFriction( static_cast(rollingFriction)); @@ -1317,63 +1315,6 @@ bool SDFFeatures::AddSdfCollision( const btScalar kd = btScalar(1e14); linkInfo->collider->setContactStiffnessAndDamping(kp, kd); } - - if (linkIndexInModel >= 0) - { - 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); - - // Set static filter for collisions in - // 1) a static model - // 2) a fixed base link - // 3) a (non-base) link with zero dofs - bool isFixed = false; - if (model->body->hasFixedBase()) - { - // check if it's a base link - isFixed = std::size_t(_linkID) == - static_cast(model->body->getUserIndex()); - // check if link has zero dofs - if (!isFixed && linkInfo->indexInModel.has_value()) - { - isFixed = model->body->getLink( - linkInfo->indexInModel.value()).m_dofCount == 0; - } - } - if (isStatic || isFixed) - { - world->world->addCollisionObject( - linkInfo->collider.get(), - btBroadphaseProxy::StaticFilter, - btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - linkInfo->isStaticOrFixed = true; - - // Set collider collision flags -#if BT_BULLET_VERSION >= 307 - linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); -#endif - } - else - { - world->world->addCollisionObject( - linkInfo->collider.get(), - btBroadphaseProxy::DefaultFilter, - btBroadphaseProxy::AllFilter); - } } else if (linkInfo->shape) { @@ -1495,6 +1436,82 @@ Identity SDFFeatures::ConstructSdfJoint( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic, + btCollisionShape *_shape, const btTransform &_shapeTF) +{ + auto *linkInfo = this->ReferenceInterface(_linkID); + auto *modelInfo = this->ReferenceInterface(linkInfo->model); + auto *worldInfo = this->ReferenceInterface(modelInfo->world); + int linkIndexInModel = -1; + + if (linkInfo->indexInModel.has_value()) + linkIndexInModel = *linkInfo->indexInModel; + linkInfo->collider = std::make_unique( + modelInfo->body.get(), linkIndexInModel); + + linkInfo->shape = std::make_unique(); + + if (_shape) + linkInfo->shape->addChildShape(_shapeTF, _shape); + + linkInfo->collider->setCollisionShape(linkInfo->shape.get()); + + if (linkIndexInModel >= 0) + { + modelInfo->body->getLink(linkIndexInModel).m_collider = + linkInfo->collider.get(); + const auto p = modelInfo->body->localPosToWorld( + linkIndexInModel, btVector3(0, 0, 0)); + const auto rot = modelInfo->body->localFrameToWorld( + linkIndexInModel, btMatrix3x3::getIdentity()); + linkInfo->collider->setWorldTransform(btTransform(rot, p)); + } + else + { + modelInfo->body->setBaseCollider(linkInfo->collider.get()); + linkInfo->collider->setWorldTransform( + modelInfo->body->getBaseWorldTransform()); + } + + // Set static filter for collisions in + // 1) a static model + // 2) a fixed base link + // 3) a (non-base) link with zero dofs + bool isFixed = false; + if (modelInfo->body->hasFixedBase()) + { + // check if it's a base link + isFixed = std::size_t(_linkID) == + static_cast(modelInfo->body->getUserIndex()); + // check if link has zero dofs + if (!isFixed && linkInfo->indexInModel.has_value()) + { + isFixed = modelInfo->body->getLink( + linkInfo->indexInModel.value()).m_dofCount == 0; + } + } + if (_isStatic || isFixed) + { + worldInfo->world->addCollisionObject( + linkInfo->collider.get(), + btBroadphaseProxy::StaticFilter, + btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + linkInfo->isStaticOrFixed = true; + + // Set collider collision flags +#if BT_BULLET_VERSION >= 307 + linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); +#endif + } + else + { + worldInfo->world->addCollisionObject( + linkInfo->collider.get(), + btBroadphaseProxy::DefaultFilter, + btBroadphaseProxy::AllFilter); + } +} } // namespace bullet_featherstone } // namespace physics diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index bea5faa70..2e8d3c9df 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/bullet-featherstone/src/SDFFeatures.hh @@ -78,6 +78,14 @@ class SDFFeatures : /// \return The entity identity if constructed otherwise an invalid identity private: Identity ConstructSdfModelImpl(std::size_t _parentID, const ::sdf::Model &_sdfModel); + + /// \brief Create and initialze the link collider in link info + /// \param[in] _linkID ID of link to create the collider for + /// \param[in] _isStatic True if the link is static + /// \param[in] _shape Collision shape to attach to link + private: void CreateLinkCollider(const Identity &_linkID, + bool _isStatic, btCollisionShape *_shape = nullptr, + const btTransform &_shapeTF = btTransform::getIdentity()); }; } // namespace bullet_featherstone diff --git a/test/common_test/worlds/test.world b/test/common_test/worlds/test.world index 715aa2472..4ee56c3b2 100644 --- a/test/common_test/worlds/test.world +++ b/test/common_test/worlds/test.world @@ -313,15 +313,6 @@ 100 - - 0 0 0 -1.57 0 0 - - - 0.1 - 0.2 - - - 0 0 0 -1.57 0 0 @@ -337,13 +328,6 @@ 1 - - - - 0.1 0.1 0.1 - - -