diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 652584f90..8df941a4c 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -112,6 +112,24 @@ struct ModelInfo } }; +/// \brief Custom GzMultiBodyLinkCollider class +class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider { + using btMultiBodyLinkCollider::btMultiBodyLinkCollider; + + /// \brief Overrides base function to enable support for ignoring + /// collision with objects from other bodies if + /// btCollisionObject::setIgnoreCollisionCheck is called. + /// Note: originally btMultiBodyLinkCollider::checkCollideWithOverride + /// just returns true if the input collision object is from a + /// different body and disregards any setIgnoreCollisionCheck calls. + public: bool checkCollideWithOverride(const btCollisionObject *_co) const + override + { + return btMultiBodyLinkCollider::checkCollideWithOverride(_co) && + btCollisionObject::checkCollideWithOverride(_co); + } +}; + /// Link information is embedded inside the model, so all we need to store here /// is a reference to the model and the index of this link inside of it. struct LinkInfo @@ -120,10 +138,12 @@ struct LinkInfo std::optional indexInModel; Identity model; Eigen::Isometry3d inertiaToLinkFrame; - std::unique_ptr collider = nullptr; + std::unique_ptr collider = nullptr; std::unique_ptr shape = nullptr; std::vector collisionEntityIds = {}; std::unordered_map collisionNameToEntityId = {}; + // Link is either static, fixed to world, or has zero dofs + bool isStaticOrFixed = false; }; struct CollisionInfo diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 3604f27ca..de0be3dca 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -18,11 +18,67 @@ #include "FreeGroupFeatures.hh" #include +#include namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// +btTransform getWorldTransformForLink(btMultiBody *_body, int _linkIndex) +{ + if (_linkIndex == -1) + { + return _body->getBaseWorldTransform(); + } + else + { + btMultiBodyLinkCollider *collider = _body->getLinkCollider(_linkIndex); + return collider->getWorldTransform(); + } +} + +///////////////////////////////////////////////// +void enforceFixedConstraint( + btMultiBodyFixedConstraint *_fixedConstraint, + const std::unordered_map &_allJoints) +{ + // Update fixed constraint's child link pose to maintain a fixed transform + // from the parent link. + btMultiBody *parent = _fixedConstraint->getMultiBodyA(); + btMultiBody *child = _fixedConstraint->getMultiBodyB(); + + btTransform parentToChildTf; + parentToChildTf.setOrigin(_fixedConstraint->getPivotInA()); + parentToChildTf.setBasis(_fixedConstraint->getFrameInA()); + + int parentLinkIndex = _fixedConstraint->getLinkA(); + int childLinkIndex = _fixedConstraint->getLinkB(); + + btTransform parentLinkTf = getWorldTransformForLink(parent, parentLinkIndex); + btTransform childLinkTf = getWorldTransformForLink(child, childLinkIndex); + + btTransform expectedChildLinkTf = parentLinkTf * parentToChildTf; + btTransform childBaseTf = child->getBaseWorldTransform(); + btTransform childBaseToLink = + childBaseTf.inverse() * childLinkTf; + btTransform newChildBaseTf = + expectedChildLinkTf * childBaseToLink.inverse(); + child->setBaseWorldTransform(newChildBaseTf); + for (const auto &joint : _allJoints) + { + if (joint.second->fixedConstraint) + { + // Recursively enforce constraints where the child here is a parent to + // other constraints. + if (joint.second->fixedConstraint->getMultiBodyA() == child) + { + enforceFixedConstraint(joint.second->fixedConstraint.get(), _allJoints); + } + } + } +} + ///////////////////////////////////////////////// Identity FreeGroupFeatures::FindFreeGroupForModel( const Identity &_modelID) const @@ -33,6 +89,20 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( if (model->body->hasFixedBase()) return this->GenerateInvalidId(); + // Also reject if the model is a child of a fixed constraint + // (detachable joint) + for (const auto & joint : this->joints) + { + if (joint.second->fixedConstraint) + { + if (joint.second->fixedConstraint->getMultiBodyB() == model->body.get()) + { + return this->GenerateInvalidId(); + } + } + } + + return _modelID; } @@ -97,7 +167,25 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( { model->body->setBaseWorldTransform( convertTf(_pose * model->baseInertiaToLinkFrame.inverse())); + model->body->wakeUp(); + + for (auto & joint : this->joints) + { + if (joint.second->fixedConstraint) + { + // Only the parent body of a fixed joint can be moved but we need to + // enforce the fixed constraint by updating the child pose so it + // remains at a fixed transform from parent. We do this recursively to + // account for other constraints attached to the child. + btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA(); + if (parent == model->body.get()) + { + enforceFixedConstraint(joint.second->fixedConstraint.get(), + this->joints); + } + } + } } } diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 588a26a6a..9618bb20c 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -27,6 +28,85 @@ namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// +void makeColliderStatic(LinkInfo *_linkInfo) +{ + btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get(); + if (!childCollider) + return; + + // if link is already static or fixed, we do not need to change its + // collision flags + if (_linkInfo->isStaticOrFixed) + return; + + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (!childProxy) + return; + + childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; + childProxy->m_collisionFilterMask = + btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; +#if BT_BULLET_VERSION >= 307 + childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); +#endif +} + +///////////////////////////////////////////////// +void makeColliderDynamic(LinkInfo *_linkInfo) +{ + btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get(); + if (!childCollider) + return; + + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (!childProxy) + return; + + // If broadphase and collision object flags do not agree, the + // link was originally non-static but made static by AttachJoint + if (!_linkInfo->isStaticOrFixed && + ((childProxy->m_collisionFilterGroup & + btBroadphaseProxy::StaticFilter) > 0)) + { + childProxy->m_collisionFilterGroup = + btBroadphaseProxy::DefaultFilter; + childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter; +#if BT_BULLET_VERSION >= 307 + childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); +#endif + } +} + +///////////////////////////////////////////////// +void updateColliderFlagsRecursive(std::size_t _linkID, + const std::unordered_map> &_joints, + const std::unordered_map> &_links, + std::function _updateColliderCb) +{ + btMultiBodyFixedConstraint *fixedConstraint = nullptr; + std::size_t childLinkID = 0u; + for (const auto &joint : _joints) + { + if (!joint.second->fixedConstraint) + continue; + if (!joint.second->parentLinkID.has_value() || + joint.second->parentLinkID.value() != _linkID) + continue; + + fixedConstraint = joint.second->fixedConstraint.get(); + childLinkID = std::size_t(joint.second->childLinkID); + } + + if (!fixedConstraint) + return; + + auto childInfo = _links.at(childLinkID); + _updateColliderCb(childInfo.get()); + + updateColliderFlagsRecursive(childLinkID, _joints, _links, _updateColliderCb); +} + ///////////////////////////////////////////////// double JointFeatures::GetJointPosition( const Identity &_id, const std::size_t _dof) const @@ -341,6 +421,31 @@ Identity JointFeatures::AttachFixedJoint( if (world != nullptr && world->world) { world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get()); + jointInfo->fixedConstraint->setMaxAppliedImpulse(btScalar(1e9)); + + // Make child link static if parent is static + // This is done by updating collision flags + btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get(); + btMultiBodyLinkCollider *childCollider = linkInfo->collider.get(); + if (parentCollider && childCollider) + { + // disable collision between parent and child collider + // \todo(iche033) if self collide is true, extend this to + // disable collisions between all the links in the parent's model with + // all the links in the child's model. + parentCollider->setIgnoreCollisionCheck(childCollider, true); + childCollider->setIgnoreCollisionCheck(parentCollider, true); + + // If parent link is static or fixed, recusively update child colliders + // collision flags to be static. + if (parentLinkInfo->isStaticOrFixed && !linkInfo->isStaticOrFixed) + { + makeColliderStatic(linkInfo); + updateColliderFlagsRecursive(std::size_t(_childID), + this->joints, this->links, makeColliderStatic); + } + } + return this->GenerateIdentity(jointID, this->joints.at(jointID)); } @@ -353,6 +458,37 @@ void JointFeatures::DetachJoint(const Identity &_jointId) auto jointInfo = this->ReferenceInterface(_jointId); if (jointInfo->fixedConstraint) { + // Make links dynamic again as they were originally not static + // This is done by reverting any collision flag changes + // made in AttachJoint + auto *linkInfo = this->ReferenceInterface(jointInfo->childLinkID); + if (jointInfo->parentLinkID.has_value()) + { + auto parentLinkInfo = this->links.at(jointInfo->parentLinkID.value()); + + btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get(); + btMultiBodyLinkCollider *childCollider = linkInfo->collider.get(); + if (parentCollider && childCollider) + { + parentCollider->setIgnoreCollisionCheck(childCollider, false); + childCollider->setIgnoreCollisionCheck(parentCollider, false); + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (childProxy) + { + // Recursively make child colliders dynamic if they were originally + // not static + if (!linkInfo->isStaticOrFixed && + ((childProxy->m_collisionFilterGroup & + btBroadphaseProxy::StaticFilter) > 0)) + { + makeColliderDynamic(linkInfo); + updateColliderFlagsRecursive(std::size_t(jointInfo->childLinkID), + this->joints, this->links, makeColliderDynamic); + } + } + } + } + auto modelInfo = this->ReferenceInterface(jointInfo->model); if (modelInfo) { @@ -360,6 +496,7 @@ void JointFeatures::DetachJoint(const Identity &_jointId) world->world->removeMultiBodyConstraint(jointInfo->fixedConstraint.get()); jointInfo->fixedConstraint.reset(); jointInfo->fixedConstraint = nullptr; + modelInfo->body->wakeUp(); } } } diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 7a0e444b3..15e7125a5 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1263,7 +1263,7 @@ bool SDFFeatures::AddSdfCollision( // NOTE: Bullet does not appear to support different surface properties // for different shapes attached to the same link. - linkInfo->collider = std::make_unique( + linkInfo->collider = std::make_unique( model->body.get(), linkIndexInModel); linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); @@ -1330,6 +1330,12 @@ bool SDFFeatures::AddSdfCollision( 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 { diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 4df041547..eac823f60 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -76,6 +76,11 @@ foreach(test ${tests}) "BT_BULLET_VERSION_LE_325" ) endif() + if (bullet_version_check_VERSION VERSION_LESS_EQUAL 3.06) + target_compile_definitions(${test_executable} PRIVATE + "BT_BULLET_VERSION_LE_306" + ) + endif() if (DART_HAS_CONTACT_SURFACE_HEADER) target_compile_definitions(${test_executable} PRIVATE DART_HAS_CONTACT_SURFACE) diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index 7ac4284d5..2328cb033 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -36,6 +36,8 @@ const auto kFallingWorld = CommonTestWorld("falling.world"); const auto kFallingAddedMassWorld = CommonTestWorld("falling_added_mass.world"); const auto kGroundSdf = CommonTestWorld("ground.sdf"); const auto kJointAcrossModelsSdf = CommonTestWorld("joint_across_models.sdf"); +const auto kJointAcrossModelsFixedSdf = + CommonTestWorld("joint_across_models_fixed.sdf"); const auto kJointConstraintSdf = CommonTestWorld("joint_constraint.sdf"); const auto kMimicFastSlowPendulumsWorld = CommonTestWorld("mimic_fast_slow_pendulums_world.sdf"); diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index b19f0e980..ab643e4bf 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -14,6 +14,10 @@ * limitations under the License. * */ + +#include +#include +#include #include #include @@ -22,6 +26,7 @@ #include #include +#include #include #include "test/TestLibLoader.hh" @@ -1146,22 +1151,396 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) } // After a while, body2 should reach the ground and come to a stop - std::size_t stepCount = 0u; - const std::size_t maxNumSteps = 2000u; - while (stepCount++ < maxNumSteps) + for (std::size_t i = 0; i < 1000; ++i) + { + world->Step(output, state, input); + } + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + } +} + +///////////////////////////////////////////////// +// Attach a fixed joint between links from non-static models to a link +// from a model fixed to the world. Verify the models are not moving when +// the fixed joint is attached. +TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsFixedSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // M1 is fixed to the world + // M2 is non-static and at some distance away from M1 + // M3 is non-static and overlaps with M1 + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string modelName3{"M3"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model3 = world->GetModel(modelName3); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + auto model3Body = model3->GetLink(bodyName); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + auto frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + const gz::math::Pose3d initialModel1Pose(0, 2, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel3Pose(0.3, 2, 3.0, 0, 0, 0.0); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + EXPECT_EQ(initialModel3Pose, + gz::math::eigen3::convert(frameDataModel3Body.pose)); + + // attach the fixed joint - model1 body is the parent + auto poseParent = frameDataModel1Body.pose; + auto poseChild = frameDataModel2Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint12 = model2Body->AttachFixedJoint(model1Body); + fixedJoint12->SetTransformFromParent(poseParentChild); + + poseChild = frameDataModel3Body.pose; + poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint13 = model3Body->AttachFixedJoint(model1Body); + fixedJoint13->SetTransformFromParent(poseParentChild); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect all models to remain at initial pose + EXPECT_NEAR(initialModel1Pose.Pos().Z(), + frameDataModel1Body.pose.translation().z(), 1e-3); + EXPECT_NEAR(initialModel2Pose.Pos().Z(), + frameDataModel2Body.pose.translation().z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // So it tries to resolve overlapping bodies held together by + // a fixed joint. Increase tolerance for position. + double tol = 1e-3; +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) == "bullet-featherstone") + tol = 1e-2; +#endif + EXPECT_NEAR(initialModel3Pose.Pos().Z(), + frameDataModel3Body.pose.translation().z(), tol); + + // Expect all models to have zero velocities + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // So overlapping bodies generate non-zero velocities. +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) != "bullet-featherstone") +#endif + { + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); + } + } + + // now detach joint and expect model2 and model3 to start moving + fixedJoint12->Detach(); + fixedJoint13->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the model1 to be fixed to the world and model2 and model3 + // to start moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_GT(0.0, body2LinearVelocity.Z()); + // bullet-featherstone and dartsim has different behavior + // when detaching a joint between overlapping bodies + // dartsim: body falls after joint is detached + // bullet-featherstone: pushes bodies apart + // So here we just check for non-zero velocity +#ifdef __APPLE__ + // Disable check for dartsim plugin on homebrew. + // model3 has zero velocity in dartsim on macOS. It could be a + // change in behavior between dartsim versions. model3 overlaps + // with model1 so could be stuck together + if (this->PhysicsEngineName(name) != "dartsim") +#endif + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } + + // Test attaching fixed joint with reverse the parent and child + // relationship - model2 body is now the parent and model1 is child and + // fixed to world + poseParent = frameDataModel2Body.pose; + poseChild = frameDataModel1Body.pose; + poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint12b = model1Body->AttachFixedJoint(model2Body); + fixedJoint12b->SetTransformFromParent(poseParentChild); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expect both models to have zero velocities + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + } + + // detach joint and expect model2 to start falling again + fixedJoint12b->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // Expect the model1 to still be fixed to the world and model2 + // to start falling + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + // Negative z velocity + EXPECT_GT(0.0, body2LinearVelocity.Z()); + } + } +} + +///////////////////////////////////////////////// +// Create a chain of models by attaching them with fixed joints: +// M1 (static) -> M2 (dynamic) -> M3 (dynamic) +// Verify that M2 and M3 become static once attached to M1 +TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachChain) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsFixedSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // M1 is fixed to the world + // M2 is non-static and at some distance away from M1 + // M3 is non-static and overlaps with M1 + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string modelName3{"M3"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model3 = world->GetModel(modelName3); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + auto model3Body = model3->GetLink(bodyName); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + auto frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + const gz::math::Pose3d initialModel1Pose(0, 2, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel3Pose(0.3, 2, 3.0, 0, 0, 0.0); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + EXPECT_EQ(initialModel3Pose, + gz::math::eigen3::convert(frameDataModel3Body.pose)); + + // attach the fixed joint between M2 (dynamic) and M3 (dynamic) + auto poseParent = frameDataModel2Body.pose; + auto poseChild = frameDataModel3Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint23 = model3Body->AttachFixedJoint(model2Body); + fixedJoint23->SetTransformFromParent(poseParentChild); + + // attach the fixed joint between M1 (static) and M2 (dynamic) + // this should recusively make M2 and M3 static + poseParent = frameDataModel2Body.pose; + poseChild = frameDataModel3Body.pose; + poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint12 = model2Body->AttachFixedJoint(model1Body); + fixedJoint12->SetTransformFromParent(poseParentChild); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); - // Expected Z height of model2 is 0.75 when both boxes are stacked on top - // of each other since each is 0.5 high. - if (fabs(frameDataModel2Body.pose.translation().z() - 0.75) < 1e-2 && - fabs(frameDataModel2Body.linearVelocity.z()) < 1e-3) + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect all models to remain at initial pose + EXPECT_NEAR(initialModel1Pose.Pos().Z(), + frameDataModel1Body.pose.translation().z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // Increase tolerance for position. + double tol = 1e-3; +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) == "bullet-featherstone") + tol = 0.1; +#endif + EXPECT_NEAR(initialModel2Pose.Pos().Z(), + frameDataModel2Body.pose.translation().z(), tol); + EXPECT_NEAR(initialModel3Pose.Pos().Z(), + frameDataModel3Body.pose.translation().z(), tol); + + // Expect all models to have zero velocities + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // So bodies generate non-zero velocities. +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) != "bullet-featherstone") +#endif { - break; + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); } } - EXPECT_GT(stepCount, 1u); - EXPECT_LT(stepCount, maxNumSteps); + + // Now detach joint between M2 (dynamic) and M3 (dynamic) + // Expect M2 to be static as it is still attached to M1 (static) + // Expect M3 to start moving + fixedJoint23->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the model1 to be fixed to the world and model2 and model3 + // to start moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-2); + // bullet-featherstone and dartsim has different behavior + // when detaching a joint between overlapping bodies + // dartsim: body falls after joint is detached + // bullet-featherstone: pushes bodies apart + // So here we just check for non-zero velocity +#ifdef __APPLE__ + // Disable check for dartsim plugin on homebrew. + // model3 has zero velocity in dartsim on macOS. It could be a + // change in behavior between dartsim versions. model3 overlaps + // with model1 so could be stuck together + if (this->PhysicsEngineName(name) != "dartsim") +#endif + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } + + // Now detach joint between M1 (static) and M2 (dynamic) + // Expect M2 to start falling + // Expect M3 to continue moving + fixedJoint12->Detach(); + // fixedJoint13->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the model1 to be fixed to the world and model2 and model3 + // to start moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_GT(0.0, body2LinearVelocity.Z()); + // bullet-featherstone and dartsim has different behavior + // when detaching a joint between overlapping bodies + // dartsim: body falls after joint is detached + // bullet-featherstone: pushes bodies apart + // So here we just check for non-zero velocity +#ifdef __APPLE__ + // Disable check for dartsim plugin on homebrew. + // model3 has zero velocity in dartsim on macOS. It could be a + // change in behavior between dartsim versions. model3 overlaps + // with model1 so could be stuck together + if (this->PhysicsEngineName(name) != "dartsim") +#endif + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } } } @@ -1269,8 +1648,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) EXPECT_EQ(initialModel3Pose, gz::math::eigen3::convert(frameDataModel3Body.pose)); - // Step through initial transients const std::size_t numSteps = 100; + /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); @@ -1283,18 +1662,15 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect all the bodies to be at rest. // (since they're held in place by the joints) - { - world->Step(output, state, input); - EXPECT_NEAR(initialModel1Pose.Z(), - frameDataModel1Body.pose.translation().z(), 1e-3); - EXPECT_NEAR(initialModel2Pose.Z(), - frameDataModel2Body.pose.translation().z(), 1e-3); - EXPECT_NEAR(initialModel3Pose.Z(), - frameDataModel3Body.pose.translation().z(), 1e-3); - EXPECT_NEAR(0.0, frameDataModel1Body.linearVelocity.z(), 3e-3); - EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); - EXPECT_NEAR(0.0, frameDataModel3Body.linearVelocity.z(), 3e-3); - } + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); } // Detach the joints. M1 and M3 should fall as there is now nothing stopping @@ -1302,13 +1678,6 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) fixedJoint_m2m1->Detach(); fixedJoint_m2m3->Detach(); - frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); - frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); - const double preStepBody1LinearVelocityZ = - frameDataModel1Body.linearVelocity.z(); - const double preStepBody3LinearVelocityZ = - frameDataModel3Body.linearVelocity.z(); - /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { @@ -1322,11 +1691,16 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect the middle box to be still as it is already at rest. // Expect the two side boxes to fall away. - EXPECT_NEAR(preStepBody1LinearVelocityZ + dt * (numSteps) * -10, - frameDataModel1Body.linearVelocity.z(), 1e-3); - EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); - EXPECT_NEAR(preStepBody3LinearVelocityZ + dt * (numSteps) * -10, - frameDataModel3Body.linearVelocity.z(), 1e-3); + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + + EXPECT_NEAR(dt * (numSteps) * -10, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(dt * (numSteps) * -10, body3LinearVelocity.Z(), 1e-3); } } } @@ -1638,6 +2012,124 @@ TEST_F(WorldModelTest, JointSetCommand) } } +using FixedJointFreeGroupFeatureList = gz::physics::FeatureList< + gz::physics::FindFreeGroupFeature, + gz::physics::SetFreeGroupWorldPose, + gz::physics::AttachFixedJointFeature, + gz::physics::DetachJointFeature, + gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, + gz::physics::LinkFrameSemantics, + gz::physics::SetBasicJointState, + gz::physics::SetJointTransformFromParentFeature, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld +>; + +using FixedJointFreeGroupFeatureTestTypes = + JointFeaturesTest; + +TEST_F(FixedJointFreeGroupFeatureTestTypes, FixedJointFreeGroupMove) +{ + // Attach joint between links from 2 models with fixed joint. Move + // parent model using free group and verify child body moves along with the + // parent link. + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = + gz::physics::RequestEngine3d::From( + plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + + const gz::math::Pose3d model1Pose(0, 0, 0.25, 0, 0, 0.0); + const gz::math::Pose3d model2Pose(0, 1, 0.25, 0, 0, 0.0); + auto freeGroupM1 = model1->FindFreeGroup(); + auto freeGroupM2 = model2->FindFreeGroup(); + freeGroupM1->SetWorldPose(gz::math::eigen3::convert(model1Pose)); + freeGroupM2->SetWorldPose(gz::math::eigen3::convert(model2Pose)); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + EXPECT_EQ(model1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(model2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expect the model1 and model2 to stay at rest + // (since they are on the ground) + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-2); + } + + // Attach fixed joint + const auto poseParent = frameDataModel1Body.pose; + const auto poseChild = frameDataModel2Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint = model2Body->AttachFixedJoint(model1Body); + fixedJoint->SetTransformFromParent(poseParentChild); + + gz::math::Pose3d poseToMoveModel(1, 0, 0, 0, 0, 0.0); + gz::math::Pose3d newModel1Pose = poseToMoveModel * model1Pose; + gz::math::Pose3d newModel2Pose = poseToMoveModel * model2Pose; + + // Move parent model using free group + freeGroupM1->SetWorldPose(gz::math::eigen3::convert(newModel1Pose)); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // Parent should be at the new pose and the child should follow + EXPECT_EQ(newModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(newModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/worlds/joint_across_models_fixed.sdf b/test/common_test/worlds/joint_across_models_fixed.sdf new file mode 100644 index 000000000..a6e355550 --- /dev/null +++ b/test/common_test/worlds/joint_across_models_fixed.sdf @@ -0,0 +1,145 @@ + + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 3.0 0 0.0 0.0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + world + body + + + + 0 0 3.0 0 0.0 0.0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0.3 2 3.0 0 0.0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + +