diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index d2dda2bb3..5c037913d 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -78,6 +78,9 @@ struct WorldInfo double stepSize = 0.001; + bool gravityEnabled = true; + btVector3 gravity = {0, 0, -9.8}; + explicit WorldInfo(std::string name); }; @@ -141,6 +144,8 @@ struct ModelInfo Eigen::Isometry3d rootLinkToModelTf; Eigen::Isometry3d baseInertiaToLinkFrame; std::shared_ptr body; + double baseMass = 0.0; + btVector3 baseInertia = btVector3(0, 0, 0); bool isNestedModel = false; @@ -197,6 +202,8 @@ struct LinkInfo std::optional indexInModel; Identity model; Eigen::Isometry3d inertiaToLinkFrame; + double mass = 0.0; + Eigen::Vector3d inertia = Eigen::Vector3d::Zero(); std::unique_ptr collider = nullptr; std::unique_ptr shape = nullptr; std::vector collisionEntityIds = {}; diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 6613ff343..57d19777f 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -24,6 +24,56 @@ namespace gz { namespace physics { namespace bullet_featherstone { +namespace +{ +///////////////////////////////////////////////// +void MakeLinkColliderStatic(LinkInfo *_linkInfo) +{ + if (!_linkInfo || !_linkInfo->collider) + return; + + if (_linkInfo->isStaticOrFixed) + return; + + btBroadphaseProxy *proxy = _linkInfo->collider->getBroadphaseHandle(); + if (!proxy) + return; + + proxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; + proxy->m_collisionFilterMask = + btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; +#if BT_BULLET_VERSION >= 307 + _linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); +#endif +} + +///////////////////////////////////////////////// +void MakeLinkColliderDynamic(LinkInfo *_linkInfo) +{ + if (!_linkInfo || !_linkInfo->collider) + return; + + btBroadphaseProxy *proxy = _linkInfo->collider->getBroadphaseHandle(); + if (!proxy) + return; + + if (_linkInfo->isStaticOrFixed) + return; + + if ((proxy->m_collisionFilterGroup & + btBroadphaseProxy::StaticFilter) == 0) + { + return; + } + + proxy->m_collisionFilterGroup = btBroadphaseProxy::DefaultFilter; + proxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter; +#if BT_BULLET_VERSION >= 307 + _linkInfo->collider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); +#endif +} +} // namespace + ///////////////////////////////////////////////// btTransform getWorldTransformForLink(btMultiBody *_body, int _linkIndex) { @@ -138,8 +188,11 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); - if (model) + if (model && model->body) { + // Do not set velocity on a static (fixed-base) model + if (model->body->hasFixedBase()) + return; model->body->setBaseOmega(convertVec(_angularVelocity)); model->body->wakeUp(); } @@ -152,13 +205,168 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); // Set Base Vel - if (model) + if (model && model->body) { + // Do not set velocity on a static (fixed-base) model + if (model->body->hasFixedBase()) + return; model->body->setBaseVel(convertVec(_linearVelocity)); model->body->wakeUp(); } } +///////////////////////////////////////////////// +void FreeGroupFeatures::SetFreeGroupGravityEnabled( + const Identity &_groupID, bool _enabled) +{ + auto *model = this->ReferenceInterface(_groupID); + if (!model) + return; + + if (model->body) + { + auto *worldInfo = this->ReferenceInterface(model->world); + if (worldInfo && worldInfo->world) + { + if (worldInfo->gravityEnabled) + { + worldInfo->gravity = worldInfo->world->getGravity(); + } + if (_enabled) + { + worldInfo->world->setGravity(worldInfo->gravity); + } + else + { + worldInfo->world->setGravity(btVector3(0, 0, 0)); + } + worldInfo->gravityEnabled = _enabled; + } + model->body->wakeUp(); + } + + for (const auto nestedModelID : model->nestedModelEntityIds) + { + auto nestedModelIt = this->models.find(nestedModelID); + if (nestedModelIt == this->models.end()) + continue; + auto nestedIdentity = this->GenerateIdentity( + nestedModelID, nestedModelIt->second); + this->SetFreeGroupGravityEnabled(nestedIdentity, _enabled); + } +} + +///////////////////////////////////////////////// +bool FreeGroupFeatures::GetFreeGroupGravityEnabled( + const Identity &_groupID) const +{ + const auto *model = this->ReferenceInterface(_groupID); + if (!model) + return false; + + bool gravityEnabled = false; + auto *worldInfo = this->ReferenceInterface(model->world); + if (worldInfo) + { + gravityEnabled = worldInfo->gravityEnabled; + } + + for (const auto nestedModelID : model->nestedModelEntityIds) + { + auto nestedModelIt = this->models.find(nestedModelID); + if (nestedModelIt == this->models.end()) + continue; + auto nestedIdentity = this->GenerateIdentity( + nestedModelID, nestedModelIt->second); + gravityEnabled = gravityEnabled && + this->GetFreeGroupGravityEnabled(nestedIdentity); + } + + return gravityEnabled; +} + +///////////////////////////////////////////////// +bool FreeGroupFeatures::GetFreeGroupStaticState( + const Identity &_groupID) const +{ + const auto *model = this->ReferenceInterface(_groupID); + if (!model) + return false; + + bool isStatic = true; + if (model->body) + isStatic = model->body->hasFixedBase(); + + for (const auto nestedModelID : model->nestedModelEntityIds) + { + auto nestedModelIt = this->models.find(nestedModelID); + if (nestedModelIt == this->models.end()) + continue; + auto nestedIdentity = this->GenerateIdentity( + nestedModelID, nestedModelIt->second); + isStatic = isStatic && + this->GetFreeGroupStaticState(nestedIdentity); + } + + return isStatic; +} + +///////////////////////////////////////////////// +void FreeGroupFeatures::SetFreeGroupStaticState( + const Identity &_groupID, + bool _enabled) +{ + auto *model = this->ReferenceInterface(_groupID); + if (!model) + return; + + if (model->body) + { + // setFixedBase toggles the fixed-base flag and updates the base + // collider's dynamic type (CF_STATIC_OBJECT / CF_DYNAMIC_OBJECT). + // The Featherstone ABA checks this flag every step, so no need to + // remove/re-add the multibody from the world. + model->body->setFixedBase(_enabled); + + if (_enabled) + { + // btMultiBody::stepPositionsMultiDof still integrates existing + // base velocities even for a static base (it only guards on + // isBaseKinematic, not isBaseStaticOrKinematic). Zero them + // explicitly so the body doesn't drift. + model->body->setBaseVel(btVector3(0, 0, 0)); + model->body->setBaseOmega(btVector3(0, 0, 0)); + model->body->clearForcesAndTorques(); + model->body->clearConstraintForces(); + model->body->clearVelocities(); + } + + model->body->wakeUp(); + } + + for (const auto linkID : model->linkEntityIds) + { + auto linkIt = this->links.find(linkID); + if (linkIt == this->links.end()) + continue; + + if (_enabled) + MakeLinkColliderStatic(linkIt->second.get()); + else + MakeLinkColliderDynamic(linkIt->second.get()); + } + + for (const auto nestedModelID : model->nestedModelEntityIds) + { + auto nestedModelIt = this->models.find(nestedModelID); + if (nestedModelIt == this->models.end()) + continue; + auto nestedIdentity = this->GenerateIdentity( + nestedModelID, nestedModelIt->second); + this->SetFreeGroupStaticState(nestedIdentity, _enabled); + } +} + ///////////////////////////////////////////////// void FreeGroupFeatures::SetFreeGroupWorldPose( const Identity &_groupID, diff --git a/bullet-featherstone/src/FreeGroupFeatures.hh b/bullet-featherstone/src/FreeGroupFeatures.hh index 30ec17d62..0bd06f84a 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.hh +++ b/bullet-featherstone/src/FreeGroupFeatures.hh @@ -48,6 +48,20 @@ class FreeGroupFeatures const Identity &_groupID, const PoseType &_pose) override; + void SetFreeGroupStaticState( + const Identity &_groupID, + bool _state) override; + + bool GetFreeGroupStaticState( + const Identity &_groupID) const override; + + void SetFreeGroupGravityEnabled( + const Identity &_groupID, + bool _enabled) override; + + bool GetFreeGroupGravityEnabled( + const Identity &_groupID) const override; + // ----- SetFreeGroupWorldVelocity ----- void SetFreeGroupWorldLinearVelocity( const Identity &_groupID, diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 7db2ceb77..8993d1353 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -617,9 +617,9 @@ Identity SDFFeatures::ConstructSdfModelImpl( }); rootMultiBody->setUserIndex(std::size_t(rootID)); - auto modelID = + auto modelID = this->GenerateIdentity(rootModelID, this->models[rootModelID]); - const auto *model = this->ReferenceInterface(modelID); + auto *model = this->ReferenceInterface(modelID); // Add world joint if (structure.rootJoint) @@ -664,6 +664,15 @@ Identity SDFFeatures::ConstructSdfModelImpl( linkIDs.insert(std::make_pair(link, linkID)); } + if (auto *linkInfo = this->ReferenceInterface(linkIDs.at(link))) + { + linkInfo->mass = static_cast(mass); + linkInfo->inertia = Eigen::Vector3d( + static_cast(inertia[0]), + static_cast(inertia[1]), + static_cast(inertia[2])); + } + if (!structure.parentOf.empty()) { const auto &parentInfo = structure.parentOf.at(link); @@ -912,6 +921,13 @@ Identity SDFFeatures::ConstructSdfModelImpl( extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose); model->body->setBaseMass(mass); model->body->setBaseInertia(inertia); + model->baseMass = mass; + model->baseInertia = inertia; + if (auto *linkInfo = this->ReferenceInterface(rootID)) + { + linkInfo->mass = static_cast(mass); + linkInfo->inertia = Eigen::Vector3d(inertia[0], inertia[1], inertia[2]); + } } world->world->addMultiBody(model->body.get()); diff --git a/bullet/src/FreeGroupFeatures.cc b/bullet/src/FreeGroupFeatures.cc index 930d97fb8..fbf79c24c 100644 --- a/bullet/src/FreeGroupFeatures.cc +++ b/bullet/src/FreeGroupFeatures.cc @@ -17,10 +17,17 @@ #include "FreeGroupFeatures.hh" +#include + namespace gz { namespace physics { namespace bullet { +namespace +{ +std::unordered_map gravityStates; +} // namespace + ///////////////////////////////////////////////// Identity FreeGroupFeatures::FindFreeGroupForModel( const Identity &_modelID) const @@ -54,9 +61,118 @@ Identity FreeGroupFeatures::FindFreeGroupForLink( Identity FreeGroupFeatures::GetFreeGroupRootLink( const Identity &_groupID) const { - (void) _groupID; - // Todo(Lobotuerk) implement canonical links - return this->GenerateInvalidId(); + const auto *model = this->ReferenceInterface(_groupID); + if (model == nullptr || model->links.empty()) + return this->GenerateInvalidId(); + + const auto rootLinkID = model->links.front(); + auto linkIt = this->links.find(rootLinkID); + if (linkIt == this->links.end() || linkIt->second == nullptr) + return this->GenerateInvalidId(); + + return this->GenerateIdentity(rootLinkID, linkIt->second); +} + +///////////////////////////////////////////////// +void FreeGroupFeatures::SetFreeGroupGravityEnabled( + const Identity &_groupID, bool _enabled) +{ + auto *model = this->ReferenceInterface(_groupID); + if (model == nullptr) + return; + + btVector3 gravity(0, 0, 0); + if (_enabled) + { + auto worldIt = this->worlds.find(static_cast(model->world)); + if (worldIt != this->worlds.end() && worldIt->second && + worldIt->second->world) + { + gravity = worldIt->second->world->getGravity(); + } + } + + for (const auto linkID : model->links) + { + auto linkIt = this->links.find(linkID); + if (linkIt == this->links.end() || !linkIt->second || !linkIt->second->link) + continue; + + auto body = linkIt->second->link.get(); + body->setGravity(gravity); + if (!_enabled) + { + // Zero out velocities and forces so the body doesn't drift + // from residual momentum when gravity is disabled + body->setLinearVelocity(btVector3(0, 0, 0)); + body->setAngularVelocity(btVector3(0, 0, 0)); + body->clearForces(); + } + body->setActivationState(ACTIVE_TAG); + } + + gravityStates[static_cast(_groupID)] = _enabled; +} + +///////////////////////////////////////////////// +bool FreeGroupFeatures::GetFreeGroupStaticState( + const Identity &_groupID) const +{ + auto *model = this->ReferenceInterface(_groupID); + if (model == nullptr) + return false; + + return model->fixed; +} + +///////////////////////////////////////////////// +void FreeGroupFeatures::SetFreeGroupStaticState( + const Identity &_groupID, + bool _enabled) +{ + auto *model = this->ReferenceInterface(_groupID); + if (model == nullptr) + return; + + model->fixed = _enabled; + for (const auto linkID : model->links) + { + auto linkIt = this->links.find(linkID); + if (linkIt == this->links.end() || !linkIt->second || !linkIt->second->link) + continue; + + auto body = linkIt->second->link.get(); + if (_enabled) + { + body->setMassProps(0, btVector3(0, 0, 0)); + body->setCollisionFlags( + body->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT); + body->setLinearVelocity(btVector3(0, 0, 0)); + body->setAngularVelocity(btVector3(0, 0, 0)); + body->clearForces(); + body->setActivationState(DISABLE_SIMULATION); + } + else + { + body->setCollisionFlags( + body->getCollisionFlags() & ~btCollisionObject::CF_STATIC_OBJECT); + // Restore mass/inertia from this link's own LinkInfo + body->setMassProps(linkIt->second->mass, linkIt->second->inertia); + body->updateInertiaTensor(); + body->setActivationState(ACTIVE_TAG); + } + } +} + +///////////////////////////////////////////////// +bool FreeGroupFeatures::GetFreeGroupGravityEnabled( + const Identity &_groupID) const +{ + auto it = gravityStates.find(static_cast(_groupID)); + if (it != gravityStates.end()) + return it->second; + + return true; } ///////////////////////////////////////////////// @@ -64,18 +180,113 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const Identity &_groupID, const PoseType &_pose) { - // Convert pose - const auto poseTranslation = _pose.translation(); - const auto poseLinear = _pose.linear(); - btTransform baseTransform; - baseTransform.setOrigin(convertVec(poseTranslation)); - baseTransform.setBasis(convertMat(poseLinear)); - - // Set base transform const auto &model = this->models.at(_groupID); - for (auto link : model->links) + if (model->links.empty()) + return; + + // Get the current world pose of the root link (link frame, not COM frame) + const auto rootLinkID = model->links.front(); + const auto &rootLinkInfo = this->links.at(rootLinkID); + btTransform currentRootCOM = + rootLinkInfo->link->getCenterOfMassTransform(); + + // Root link world pose = currentRootCOM * inertialPose.Inverse() + const auto rootInertialPose = + gz::math::eigen3::convert(rootLinkInfo->inertialPose); + btTransform rootInertialBt; + rootInertialBt.setOrigin(convertVec(rootInertialPose.translation())); + rootInertialBt.setBasis(convertMat(rootInertialPose.linear())); + + btTransform currentRootWorldPose = currentRootCOM * rootInertialBt.inverse(); + + // Desired new world pose for the root link + btTransform newRootWorldPose; + newRootWorldPose.setOrigin(convertVec(_pose.translation())); + newRootWorldPose.setBasis(convertMat(_pose.linear())); + + // Compute the relative transform change + btTransform tfChange = newRootWorldPose * currentRootWorldPose.inverse(); + + // Apply the transform change to all links + for (const auto linkID : model->links) { - this->links.at(link)->link->setCenterOfMassTransform(baseTransform); + const auto &linkInfo = this->links.at(linkID); + auto body = linkInfo->link.get(); + + // Current link world pose (link frame) + btTransform linkInertialBt; + const auto linkInertialPose = + gz::math::eigen3::convert(linkInfo->inertialPose); + linkInertialBt.setOrigin(convertVec(linkInertialPose.translation())); + linkInertialBt.setBasis(convertMat(linkInertialPose.linear())); + + btTransform currentLinkCOM = body->getCenterOfMassTransform(); + btTransform currentLinkWorld = currentLinkCOM * linkInertialBt.inverse(); + + // Apply the change to get new link world pose, then convert back to COM + btTransform newLinkWorld = tfChange * currentLinkWorld; + btTransform newLinkCOM = newLinkWorld * linkInertialBt; + + body->setCenterOfMassTransform(newLinkCOM); + // Also update the motion state so Bullet doesn't reset the pose + if (body->getMotionState()) + { + body->getMotionState()->setWorldTransform(newLinkCOM); + } + body->setLinearVelocity(btVector3(0, 0, 0)); + body->setAngularVelocity(btVector3(0, 0, 0)); + body->clearForces(); + body->setActivationState(ACTIVE_TAG); + } +} + +void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( + const Identity &_groupID, + const LinearVelocity &_linearVelocity) +{ + auto *model = this->ReferenceInterface(_groupID); + if (model == nullptr) + return; + + // Do not set velocity on a static model + if (model->fixed) + return; + + const btVector3 velocity = convertVec(_linearVelocity); + for (const auto linkID : model->links) + { + auto linkIt = this->links.find(linkID); + if (linkIt == this->links.end() || !linkIt->second || !linkIt->second->link) + continue; + + auto body = linkIt->second->link.get(); + body->setLinearVelocity(velocity); + body->setActivationState(ACTIVE_TAG); + } +} + +void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( + const Identity &_groupID, + const AngularVelocity &_angularVelocity) +{ + auto *model = this->ReferenceInterface(_groupID); + if (model == nullptr) + return; + + // Do not set velocity on a static model + if (model->fixed) + return; + + const btVector3 angular = convertVec(_angularVelocity); + for (const auto linkID : model->links) + { + auto linkIt = this->links.find(linkID); + if (linkIt == this->links.end() || !linkIt->second || !linkIt->second->link) + continue; + + auto body = linkIt->second->link.get(); + body->setAngularVelocity(angular); + body->setActivationState(ACTIVE_TAG); } } diff --git a/bullet/src/FreeGroupFeatures.hh b/bullet/src/FreeGroupFeatures.hh index 278f066b5..5b26c7f13 100644 --- a/bullet/src/FreeGroupFeatures.hh +++ b/bullet/src/FreeGroupFeatures.hh @@ -27,8 +27,13 @@ namespace physics { namespace bullet { struct FreeGroupFeatureList : gz::physics::FeatureList< + GetFreeGroupStaticState, + GetFreeGroupGravityEnabled, FindFreeGroupFeature, - SetFreeGroupWorldPose + SetFreeGroupWorldPose, + SetFreeGroupWorldVelocity, + SetFreeGroupStaticState, + SetFreeGroupGravityEnabled > { }; class FreeGroupFeatures @@ -42,10 +47,33 @@ class FreeGroupFeatures Identity GetFreeGroupRootLink(const Identity &_groupID) const override; + void SetFreeGroupStaticState( + const Identity &_groupID, + bool _state) override; + + bool GetFreeGroupStaticState( + const Identity &_groupID) const override; + + void SetFreeGroupGravityEnabled( + const Identity &_groupID, + bool _enabled) override; + + bool GetFreeGroupGravityEnabled( + const Identity &_groupID) const override; + // ----- SetFreeGroupWorldPose ----- void SetFreeGroupWorldPose( const Identity &_groupID, const PoseType &_pose) override; + + // ----- SetFreeGroupWorldVelocity ----- + void SetFreeGroupWorldLinearVelocity( + const Identity &_groupID, + const LinearVelocity &_linearVelocity) override; + + void SetFreeGroupWorldAngularVelocity( + const Identity &_groupID, + const AngularVelocity &_angularVelocity) override; }; } // namespace bullet diff --git a/bullet/src/KinematicsFeatures.cc b/bullet/src/KinematicsFeatures.cc index 38057c3d3..4d3243fa7 100644 --- a/bullet/src/KinematicsFeatures.cc +++ b/bullet/src/KinematicsFeatures.cc @@ -37,12 +37,22 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( return data; } - const auto linkID = _id.ID(); + auto linkID = _id.ID(); + // If the ID doesn't correspond to a link, it may be a model (e.g. from a + // FreeGroup). Resolve to the model's root link. if (this->links.find(linkID) == this->links.end()) { - gzerr << "Given a FrameID not belonging to a link.\n"; - return data; + const auto modelIt = this->models.find(linkID); + if (modelIt != this->models.end() && !modelIt->second->links.empty()) + { + linkID = modelIt->second->links.front(); + } + else + { + gzerr << "Given a FrameID not belonging to a link or model.\n"; + return data; + } } const auto &linkInfo = this->links.at(linkID); const auto &rigidBody = linkInfo->link; diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 1b4c5a392..8e6dcdc68 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -1285,7 +1285,6 @@ TYPED_TEST(SimulationFeaturesTestFreeGroupPhysics, FreeGroup) for (const std::string &name : this->pluginNames) { CHECK_UNSUPPORTED_ENGINE(name, "tpe") - CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") auto world = LoadPluginAndWorld( this->loader, @@ -1396,7 +1395,6 @@ TYPED_TEST(SimulationFeaturesTestFreeGroupPhysics, FreeGroupNested) for (const std::string &name : this->pluginNames) { CHECK_UNSUPPORTED_ENGINE(name, "tpe") - CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") auto world = LoadPluginAndWorld( this->loader,