From 894a348c9e24efd78148e4013de5c3eaa1e41db2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 6 May 2024 22:41:39 +0000 Subject: [PATCH 01/25] Add FixedJointWeldChildToParent feature and update bullet-featherstone fixed joint behavior Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 6 + bullet-featherstone/src/JointFeatures.cc | 60 +++ bullet-featherstone/src/JointFeatures.hh | 6 +- bullet-featherstone/src/SDFFeatures.cc | 1 + bullet-featherstone/src/SimulationFeatures.cc | 58 +++ include/gz/physics/Joint.hh | 31 ++ include/gz/physics/detail/Joint.hh | 19 + test/common_test/Worlds.hh | 2 + test/common_test/joint_features.cc | 398 ++++++++++++++++-- .../worlds/joint_across_models_fixed.sdf | 145 +++++++ 10 files changed, 690 insertions(+), 36 deletions(-) create mode 100644 test/common_test/worlds/joint_across_models_fixed.sdf diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 652584f90..935bfaae9 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -124,6 +124,8 @@ struct LinkInfo 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 @@ -171,6 +173,10 @@ struct JointInfo std::size_t indexInGzModel = 0; btScalar effort = 0; + // True if the fixed constraint's child link is welded to parent link as if + // they are part of the same body. + bool fixedConstraintWeldChildToParent = true; + std::shared_ptr motor = nullptr; std::shared_ptr jointLimits = nullptr; std::shared_ptr fixedConstraint = nullptr; diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 588a26a6a..deb9f9e98 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -341,6 +341,26 @@ Identity JointFeatures::AttachFixedJoint( if (world != nullptr && world->world) { world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get()); + + // Make links static if parent or child link is static + // This is done by changing collision filter groups / masks + // Otherwise bullet will push bodies apart + btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get(); + btMultiBodyLinkCollider *childCollider = linkInfo->collider.get(); + if (parentCollider && childCollider) + { + if (parentLinkInfo->isStaticOrFixed && !linkInfo->isStaticOrFixed) + { + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (childProxy) + { + childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; + childProxy->m_collisionFilterMask = + btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; + } + } + } + return this->GenerateIdentity(jointID, this->joints.at(jointID)); } @@ -353,6 +373,33 @@ void JointFeatures::DetachJoint(const Identity &_jointId) auto jointInfo = this->ReferenceInterface(_jointId); if (jointInfo->fixedConstraint) { + // Make links dynamic again they were originally not static + // This is done by revert any collision flags / masks 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) + { + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (childProxy) + { + // 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; + } + } + } + } + auto modelInfo = this->ReferenceInterface(jointInfo->model); if (modelInfo) { @@ -360,6 +407,7 @@ void JointFeatures::DetachJoint(const Identity &_jointId) world->world->removeMultiBodyConstraint(jointInfo->fixedConstraint.get()); jointInfo->fixedConstraint.reset(); jointInfo->fixedConstraint = nullptr; + modelInfo->body->wakeUp(); } } } @@ -380,6 +428,18 @@ void JointFeatures::SetJointTransformFromParent( } } +///////////////////////////////////////////////// +void JointFeatures::SetFixedJointWeldChildToParent( + const Identity &_id, bool _weldChildToParent) +{ + auto jointInfo = this->ReferenceInterface(_id); + + if (jointInfo->fixedConstraint) + { + jointInfo->fixedConstraintWeldChildToParent = _weldChildToParent; + } +} + ///////////////////////////////////////////////// Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( const Identity &_id) const diff --git a/bullet-featherstone/src/JointFeatures.hh b/bullet-featherstone/src/JointFeatures.hh index bc6c274f8..42740fbf3 100644 --- a/bullet-featherstone/src/JointFeatures.hh +++ b/bullet-featherstone/src/JointFeatures.hh @@ -51,7 +51,7 @@ struct JointFeatureList : FeatureList< SetMimicConstraintFeature, - FixedJointCast + SetFixedJointWeldChildToParentFeature > { }; class JointFeatures : @@ -140,6 +140,10 @@ class JointFeatures : public: void SetJointTransformFromParent( const Identity &_id, const Pose3d &_pose) override; + // ----- SetFixedJointWeldChildToParentFeature ----- + public: void SetFixedJointWeldChildToParent( + const Identity &_id, bool _weldChildToParent) override; + // ----- Transmitted wrench ----- public: Wrench3d GetJointTransmittedWrenchInJointFrame( const Identity &_id) const override; diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 0679fb831..b72315bc3 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1328,6 +1328,7 @@ bool SDFFeatures::AddSdfCollision( linkInfo->collider.get(), btBroadphaseProxy::StaticFilter, btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + linkInfo->isStaticOrFixed = true; } else { diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index a32d93d2c..a7f4b35de 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -43,6 +43,64 @@ void SimulationFeatures::WorldForwardStep( stepSize = dt.count(); } + // Update fixed constraint behavior if fixedJointWeldChildToParent is true. + // Do this before stepping, i.e. before physics engine tries to solve and + // enforce the constraint + for (auto & joint : this->joints) + { + if (joint.second->fixedConstraint && + joint.second->fixedConstraintWeldChildToParent) + { + // Update fxied constraint's child link pose to maintain a fixed transform + // from the parent link. + btTransform parentToChildTf; + parentToChildTf.setOrigin(joint.second->fixedConstraint->getPivotInA()); + parentToChildTf.setBasis(joint.second->fixedConstraint->getFrameInA()); + btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA(); + btMultiBody *child = joint.second->fixedConstraint->getMultiBodyB(); + + int parentLinkIndex = joint.second->fixedConstraint->getLinkA(); + int childLinkIndex = joint.second->fixedConstraint->getLinkB(); + + btTransform parentLinkTf; + btTransform childLinkTf; + if (parentLinkIndex == -1) + { + parentLinkTf = parent->getBaseWorldTransform(); + } + else + { + btMultiBodyLinkCollider *parentCollider = + parent->getLinkCollider(parentLinkIndex); + parentLinkTf = parentCollider->getWorldTransform(); + } + if (childLinkIndex == -1) + { + childLinkTf = child->getBaseWorldTransform(); + } + else + { + btMultiBodyLinkCollider *childCollider = + child->getLinkCollider(childLinkIndex); + childLinkTf = childCollider->getWorldTransform(); + } + btTransform expectedChildLinkTf = parentLinkTf * parentToChildTf; + btTransform childBaseTf = child->getBaseWorldTransform(); + btTransform childBaseToLink = + childBaseTf.inverse() * childLinkTf; + btTransform newChildBaseTf = + expectedChildLinkTf * childBaseToLink.inverse(); + child->setBaseWorldTransform(newChildBaseTf); + + auto parentBaseLinkTf = parent->getBaseWorldTransform(); + btMultiBodyLinkCollider *parentCollider = + parent->getBaseCollider(); + + // parent->stepPositionsMultiDof(btScalar(0)); + parentLinkTf = parentCollider->getWorldTransform(); + } + } + worldInfo->world->stepSimulation(static_cast(this->stepSize), 1, static_cast(this->stepSize)); diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 1dfe7dc7f..45d593e99 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -652,6 +652,37 @@ namespace gz Scalar _reference) = 0; }; }; + + ///////////////////////////////////////////////// + class GZ_PHYSICS_VISIBLE SetFixedJointWeldChildToParentFeature + : public virtual Feature + { + /// \brief The Joint API for setting the transform from the joint's parent + public: template + class Joint : public virtual Feature::Joint + { + /// \brief Set whether to weld the fixed joint's child link to the + /// parent link. If true, the child link is welded and it will move / + /// with the parent link as if they are part of the same body + /// kinematic chain. If false, the fixed joint constraint is enforced + /// by applying forces to both the parent and child links. + /// By default when a fixed joint constraint is created, this property + /// is set to true. + /// \param[in] _weldChildToParent True to weld the child link to the + /// parent link. + public: void SetWeldChildToParent(bool _weldChildToParent); + }; + + /// \private The implementation API for setting the joint childtransform from the + /// parent + public: template + class Implementation : public virtual Feature::Implementation + { + // see Joint::SetWeldChildToParent above + public: virtual void SetFixedJointWeldChildToParent( + const Identity &_id, bool _weldChildToParent) = 0; + }; + }; } } diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index 801042d29..f46325584 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -263,6 +263,25 @@ namespace gz RelativeWrench(this->GetFrameID(), this->GetTransmittedWrench()), _relativeTo, _inCoordinatesOf); } + + ///////////////////////////////////////////////// + template + void SetFixedJointWeldChildToParentFeature::Joint:: + SetWeldChildToParent(bool _weldChildToParent) + { + this->template Interface() + ->SetFixedJointWeldChildToParent(this->identity, _weldChildToParent); + } + +/* ///////////////////////////////////////////////// + template + auto GetFixedJointWeldChildToParentFeature::Joint:: + GetWeldChildToParent() const -> bool + { + return this->template Interface() + ->GetFixedJointWeldChildToParent(this->identity); + } +*/ } } 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..d70640d7e 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1146,22 +1146,194 @@ 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(); - // 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); + EXPECT_NEAR(initialModel2Pose.Pos().Z(), + frameDataModel2Body.pose.translation().z(), 1e-3); + // \todo(iche033) Tolerance increased for bullet-featherstone + // as it tries to resolve overlapping bodies held together by + // a fixed joint + EXPECT_NEAR(initialModel3Pose.Pos().Z(), + frameDataModel3Body.pose.translation().z(), 1e-2); + + // 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); + // \todo(iche033) bullet-featherstone produces non-zero velocities + // for overlapping bodies + if (this->PhysicsEngineName(name) != "bullet-featherstone") { - break; + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); } } - EXPECT_GT(stepCount, 1u); - EXPECT_LT(stepCount, maxNumSteps); + + // 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 with + // 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 + 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()); + } } } @@ -1269,8 +1441,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 +1455,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 +1471,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 +1484,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 +1805,167 @@ TEST_F(WorldModelTest, JointSetCommand) } } +using FixedJointWeldFeatureList = 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::SetFixedJointWeldChildToParentFeature, + gz::physics::SetJointTransformFromParentFeature, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld +>; + +using FixedJointWeldFeatureTestTypes = + JointFeaturesTest; + +TEST_F(FixedJointWeldFeatureTestTypes, FixedJointWeldChildToParent) +{ + // Attach joint between links from 2 models with different weld child to + // parent property values and verify behavior + // When child is welded to parent, the child behave as if it's in the same + // body as the parent and 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 with child welded to parent link + // This should already be true by default + const auto poseParent = frameDataModel1Body.pose; + const auto poseChild = frameDataModel2Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint = model2Body->AttachFixedJoint(model1Body); + fixedJoint->SetTransformFromParent(poseParentChild); + fixedJoint->SetWeldChildToParent(true); + + 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 + 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)); + + // Disable weld child to parent link + fixedJoint->SetWeldChildToParent(false); + + // Move the parent model again + newModel1Pose = poseToMoveModel * newModel1Pose; + newModel2Pose = poseToMoveModel * newModel2Pose; + freeGroupM1->SetWorldPose(gz::math::eigen3::convert(newModel1Pose)); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + auto actualModel2Pose = gz::math::eigen3::convert(frameDataModel2Body.pose); + } + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // The fixed constraint pulls parent and child model towards each other + // The parent should no longer be at the pose that we set it to be. + auto actualModel1Pose = gz::math::eigen3::convert(frameDataModel1Body.pose); + auto actualModel2Pose = gz::math::eigen3::convert(frameDataModel2Body.pose); + EXPECT_NE(newModel1Pose, actualModel1Pose); + EXPECT_NE(newModel2Pose, actualModel2Pose); + + // Fixed joint constraint should maintain the parent <-> child pose offset + auto expectedModel2Pose = gz::math::eigen3::convert(poseParentChild) * + actualModel1Pose; + // \todo(iche033) Large tol used for bullet-featherstone as it does not + // completely correct position errors. Investigate ways to be more strict in + // enforcing the fixed constraint, e.g. set erp, max applied impulse, etc. + EXPECT_NEAR(expectedModel2Pose.Pos().X(), actualModel2Pose.Pos().X(), 0.2); + EXPECT_NEAR(expectedModel2Pose.Pos().Y(), actualModel2Pose.Pos().Y(), 2e-2); + EXPECT_NEAR(expectedModel2Pose.Pos().Z(), actualModel2Pose.Pos().Z(), 1e-3); + EXPECT_NEAR(expectedModel2Pose.Rot().Euler().X(), + actualModel2Pose.Rot().Euler().X(), 1e-3); + EXPECT_NEAR(expectedModel2Pose.Rot().Euler().Y(), + actualModel2Pose.Rot().Euler().Y(), 1e-3); + EXPECT_NEAR(expectedModel2Pose.Rot().Euler().Z(), + actualModel2Pose.Rot().Euler().Z(), 0.2); + } +} + 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 + + + + + + + From 4e0584441bba244c33fb1ed19ecb70f7e256424d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 6 May 2024 23:01:31 +0000 Subject: [PATCH 02/25] clean up Signed-off-by: Ian Chen --- include/gz/physics/Joint.hh | 7 ++++--- include/gz/physics/detail/Joint.hh | 10 ---------- 2 files changed, 4 insertions(+), 13 deletions(-) diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 45d593e99..85e5005e3 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -657,7 +657,8 @@ namespace gz class GZ_PHYSICS_VISIBLE SetFixedJointWeldChildToParentFeature : public virtual Feature { - /// \brief The Joint API for setting the transform from the joint's parent + /// \brief The Joint API for setting whether to weld a fixed joint's child + /// link to the parent link. public: template class Joint : public virtual Feature::Joint { @@ -673,8 +674,8 @@ namespace gz public: void SetWeldChildToParent(bool _weldChildToParent); }; - /// \private The implementation API for setting the joint childtransform from the - /// parent + /// \private The implementation API for setting whether to weld the fixed + /// joint's child link to the parent link. public: template class Implementation : public virtual Feature::Implementation { diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index f46325584..f762b0db1 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -272,16 +272,6 @@ namespace gz this->template Interface() ->SetFixedJointWeldChildToParent(this->identity, _weldChildToParent); } - -/* ///////////////////////////////////////////////// - template - auto GetFixedJointWeldChildToParentFeature::Joint:: - GetWeldChildToParent() const -> bool - { - return this->template Interface() - ->GetFixedJointWeldChildToParent(this->identity); - } -*/ } } From 4eae0c432e3b64f01709d90dc587ee75f7647949 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 6 May 2024 23:27:29 +0000 Subject: [PATCH 03/25] lint Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index deb9f9e98..0464d67cf 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -390,10 +390,12 @@ void JointFeatures::DetachJoint(const Identity &_jointId) { // 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 & + if (!linkInfo->isStaticOrFixed && + ((childProxy->m_collisionFilterGroup & btBroadphaseProxy::StaticFilter) > 0)) { - childProxy->m_collisionFilterGroup = btBroadphaseProxy::DefaultFilter; + childProxy->m_collisionFilterGroup = + btBroadphaseProxy::DefaultFilter; childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter; } } From 8e32ed8542d583143c0200708037e63cbc8fccea Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 7 May 2024 01:23:37 +0000 Subject: [PATCH 04/25] remove unused code Signed-off-by: Ian Chen --- bullet-featherstone/src/SimulationFeatures.cc | 7 ------- 1 file changed, 7 deletions(-) diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index a7f4b35de..c85916ad2 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -91,13 +91,6 @@ void SimulationFeatures::WorldForwardStep( btTransform newChildBaseTf = expectedChildLinkTf * childBaseToLink.inverse(); child->setBaseWorldTransform(newChildBaseTf); - - auto parentBaseLinkTf = parent->getBaseWorldTransform(); - btMultiBodyLinkCollider *parentCollider = - parent->getBaseCollider(); - - // parent->stepPositionsMultiDof(btScalar(0)); - parentLinkTf = parentCollider->getWorldTransform(); } } From 12cef38b14ac45819a91f0d04261e2b8b5fd6147 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 7 May 2024 03:26:05 +0000 Subject: [PATCH 05/25] disable check on mac Signed-off-by: Ian Chen --- test/common_test/joint_features.cc | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index d70640d7e..dc1a53dc3 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1278,11 +1278,18 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) 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 with + // 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. + // jodel3 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); } From 2e49b03d4203df43d6ba10e8577a8c0cfe17e04b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 7 May 2024 16:59:23 +0000 Subject: [PATCH 06/25] add includes Signed-off-by: Ian Chen --- test/common_test/joint_features.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index dc1a53dc3..35c8fd178 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" From e1c1b8424025fa649b9ee8c7f3b501acad325efd Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 10 May 2024 22:06:32 +0000 Subject: [PATCH 07/25] minor collision checking optimization Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 4 ++++ bullet-featherstone/src/SDFFeatures.cc | 10 ++++++++++ 2 files changed, 14 insertions(+) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 0464d67cf..b663c5ecf 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -349,6 +349,8 @@ Identity JointFeatures::AttachFixedJoint( btMultiBodyLinkCollider *childCollider = linkInfo->collider.get(); if (parentCollider && childCollider) { + parentCollider->setIgnoreCollisionCheck(childCollider, true); + childCollider->setIgnoreCollisionCheck(parentCollider, true); if (parentLinkInfo->isStaticOrFixed && !linkInfo->isStaticOrFixed) { btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); @@ -385,6 +387,8 @@ void JointFeatures::DetachJoint(const Identity &_jointId) btMultiBodyLinkCollider *childCollider = linkInfo->collider.get(); if (parentCollider && childCollider) { + parentCollider->setIgnoreCollisionCheck(childCollider, false); + childCollider->setIgnoreCollisionCheck(parentCollider, false); btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); if (childProxy) { diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index b72315bc3..346aeb740 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1329,6 +1329,16 @@ bool SDFFeatures::AddSdfCollision( 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); +#else + int oldFlags = linkInfo->collider->getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | + btCollisionObject::CF_KINEMATIC_OBJECT); + linkInfo->collider->setCollisionFlags(oldFlags | dynamicType); +#endif } else { From 33af03e49cddc5a82c7fa146aee63e4b6500ae54 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 10 May 2024 22:12:10 +0000 Subject: [PATCH 08/25] fixes Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 346aeb740..0346433cb 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1335,9 +1335,8 @@ bool SDFFeatures::AddSdfCollision( linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); #else int oldFlags = linkInfo->collider->getCollisionFlags(); - oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | - btCollisionObject::CF_KINEMATIC_OBJECT); - linkInfo->collider->setCollisionFlags(oldFlags | dynamicType); + linkInfo->collider->setCollisionFlags(oldFlags | + btCollisionObject::CF_STATIC_OBJECT); #endif } else From ab22dfc5116017676b26d74bde7c1e6e9894751a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 10 May 2024 23:01:06 +0000 Subject: [PATCH 09/25] further optimize collision flags Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 17 +++++++++++++++++ test/common_test/joint_features.cc | 12 ++---------- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index b663c5ecf..7662a6476 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -360,6 +360,13 @@ Identity JointFeatures::AttachFixedJoint( childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; } +#if BT_BULLET_VERSION >= 307 + childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); +#else + int oldFlags = linkInfo->collider->getCollisionFlags(); + linkInfo->collider->setCollisionFlags(oldFlags | + btCollisionObject::CF_STATIC_OBJECT); +#endif } } @@ -401,6 +408,16 @@ void JointFeatures::DetachJoint(const Identity &_jointId) childProxy->m_collisionFilterGroup = btBroadphaseProxy::DefaultFilter; childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter; + +#if BT_BULLET_VERSION >= 307 + childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); +#else + int oldFlags = linkInfo->collider->getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | + btCollisionObject::CF_KINEMATIC_OBJECT); + linkInfo->collider->setCollisionFlags(oldFlags | + btCollisionObject::CF_DYNAMIC_OBJECT); +#endif } } } diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 35c8fd178..8a7d2f155 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1239,11 +1239,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) frameDataModel1Body.pose.translation().z(), 1e-3); EXPECT_NEAR(initialModel2Pose.Pos().Z(), frameDataModel2Body.pose.translation().z(), 1e-3); - // \todo(iche033) Tolerance increased for bullet-featherstone - // as it tries to resolve overlapping bodies held together by - // a fixed joint EXPECT_NEAR(initialModel3Pose.Pos().Z(), - frameDataModel3Body.pose.translation().z(), 1e-2); + frameDataModel3Body.pose.translation().z(), 1e-3); // Expect all models to have zero velocities gz::math::Vector3d body1LinearVelocity = @@ -1254,12 +1251,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); - // \todo(iche033) bullet-featherstone produces non-zero velocities - // for overlapping bodies - if (this->PhysicsEngineName(name) != "bullet-featherstone") - { - EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); - } + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); } // now detach joint and expect model2 and model3 to start moving From 936d1643dbea6af6c01d7e5a3a3b3abc906282da Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 10 May 2024 23:02:27 +0000 Subject: [PATCH 10/25] style Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 0346433cb..7a128772c 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1334,8 +1334,8 @@ bool SDFFeatures::AddSdfCollision( #if BT_BULLET_VERSION >= 307 linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); #else - int oldFlags = linkInfo->collider->getCollisionFlags(); - linkInfo->collider->setCollisionFlags(oldFlags | + int oldFlags = linkInfo->collider->getCollisionFlags(); + linkInfo->collider->setCollisionFlags(oldFlags | btCollisionObject::CF_STATIC_OBJECT); #endif } From 19adb26fc0171dd60356f850167bffac9253f69a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 11 May 2024 00:15:04 +0000 Subject: [PATCH 11/25] style Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 7662a6476..37ea28caf 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -363,8 +363,8 @@ Identity JointFeatures::AttachFixedJoint( #if BT_BULLET_VERSION >= 307 childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); #else - int oldFlags = linkInfo->collider->getCollisionFlags(); - linkInfo->collider->setCollisionFlags(oldFlags | + int oldFlags = linkInfo->collider->getCollisionFlags(); + linkInfo->collider->setCollisionFlags(oldFlags | btCollisionObject::CF_STATIC_OBJECT); #endif } From 6c2db2606fb79834f145da369610c4efbcc775b4 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 11 May 2024 01:25:32 +0000 Subject: [PATCH 12/25] fixes for bullet version < 3.07 Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 37ea28caf..32e435d5b 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -363,8 +363,8 @@ Identity JointFeatures::AttachFixedJoint( #if BT_BULLET_VERSION >= 307 childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); #else - int oldFlags = linkInfo->collider->getCollisionFlags(); - linkInfo->collider->setCollisionFlags(oldFlags | + int oldFlags = childCollider->getCollisionFlags(); + childCollider->setCollisionFlags(oldFlags | btCollisionObject::CF_STATIC_OBJECT); #endif } @@ -412,11 +412,10 @@ void JointFeatures::DetachJoint(const Identity &_jointId) #if BT_BULLET_VERSION >= 307 childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); #else - int oldFlags = linkInfo->collider->getCollisionFlags(); + int oldFlags = childCollider->getCollisionFlags(); oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); - linkInfo->collider->setCollisionFlags(oldFlags | - btCollisionObject::CF_DYNAMIC_OBJECT); + childCollider->setCollisionFlags(oldFlags); #endif } } From c5e33e938a2ef698a6e4b0868d43a58afee2dcd2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 11 May 2024 02:55:57 +0000 Subject: [PATCH 13/25] check bullet version Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 9 --------- bullet-featherstone/src/SDFFeatures.cc | 4 ---- test/common_test/CMakeLists.txt | 5 +++++ test/common_test/joint_features.cc | 9 ++++++++- 4 files changed, 13 insertions(+), 14 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 32e435d5b..d9b1730e6 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -362,10 +362,6 @@ Identity JointFeatures::AttachFixedJoint( } #if BT_BULLET_VERSION >= 307 childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); -#else - int oldFlags = childCollider->getCollisionFlags(); - childCollider->setCollisionFlags(oldFlags | - btCollisionObject::CF_STATIC_OBJECT); #endif } } @@ -411,11 +407,6 @@ void JointFeatures::DetachJoint(const Identity &_jointId) #if BT_BULLET_VERSION >= 307 childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); -#else - int oldFlags = childCollider->getCollisionFlags(); - oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | - btCollisionObject::CF_KINEMATIC_OBJECT); - childCollider->setCollisionFlags(oldFlags); #endif } } diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 7a128772c..fdd694225 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1333,10 +1333,6 @@ bool SDFFeatures::AddSdfCollision( // Set collider collision flags #if BT_BULLET_VERSION >= 307 linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); -#else - int oldFlags = linkInfo->collider->getCollisionFlags(); - linkInfo->collider->setCollisionFlags(oldFlags | - 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/joint_features.cc b/test/common_test/joint_features.cc index 8a7d2f155..94f1d520f 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1251,7 +1251,14 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) 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); + // bullet-featherstone produces non-zero velocities + // for overlapping bodies in versions <= 3.06. +#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 From b69191a29d6e6a46a6b03c8db25f7c28a8fe6ec3 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 11 May 2024 19:10:16 +0000 Subject: [PATCH 14/25] more ifdef Signed-off-by: Ian Chen --- test/common_test/joint_features.cc | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 94f1d520f..3fa2e4f11 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1239,8 +1239,16 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) 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. + const 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(), 1e-3); + frameDataModel3Body.pose.translation().z(), tol); // Expect all models to have zero velocities gz::math::Vector3d body1LinearVelocity = @@ -1251,8 +1259,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); - // bullet-featherstone produces non-zero velocities - // for overlapping bodies in versions <= 3.06. + // 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 From c904cb54a0bd75da9cb0983835efa5803bc7fb96 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 13 May 2024 17:40:17 +0000 Subject: [PATCH 15/25] fix Signed-off-by: Ian Chen --- test/common_test/joint_features.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 3fa2e4f11..cba5f1bcc 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1242,7 +1242,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) // 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. - const double tol = 1e-3; + double tol = 1e-3; #ifdef BT_BULLET_VERSION_LE_306 if (this->PhysicsEngineName(name) == "bullet-featherstone") tol = 1e-2; From 755aeb516c1ba86d9d25c6852755148f37df1974 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 17 May 2024 01:17:50 -0500 Subject: [PATCH 16/25] Disable collisions between attached bodies (#640) Signed-off-by: Addisu Z. Taddese --- bullet-featherstone/src/Base.hh | 9 ++++++++- bullet-featherstone/src/SDFFeatures.cc | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 935bfaae9..dfe5371b8 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -112,6 +112,13 @@ struct ModelInfo } }; +class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider { + using btMultiBodyLinkCollider::btMultiBodyLinkCollider; + + 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,7 +127,7 @@ 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 = {}; diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 236e4cd99..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()); From 26a72cda7530018cb72d889a3c500a4d3403bede Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 28 May 2024 08:56:58 +0000 Subject: [PATCH 17/25] add comments Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index dfe5371b8..6d77d374b 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -112,13 +112,24 @@ struct ModelInfo } }; +/// \brief Custom GzMultiBodyLinkCollider class class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider { using btMultiBodyLinkCollider::btMultiBodyLinkCollider; - bool checkCollideWithOverride(const btCollisionObject* co) const override { - return btMultiBodyLinkCollider::checkCollideWithOverride(co) && btCollisionObject::checkCollideWithOverride(co); + /// \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 From d9b0bdb4d0ac9a1d6505f5085bfa3947f2a67c34 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 1 Jun 2024 13:15:52 +0000 Subject: [PATCH 18/25] Remove SetFixedJointWeldChildToParentFeature. Enforce fixed constraint for free group set world pose. Recursively update collider flags on attach / detach Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 4 - bullet-featherstone/src/FreeGroupFeatures.cc | 68 +++++ bullet-featherstone/src/JointFeatures.cc | 122 ++++++--- bullet-featherstone/src/JointFeatures.hh | 8 +- bullet-featherstone/src/SimulationFeatures.cc | 51 ---- include/gz/physics/Joint.hh | 32 --- include/gz/physics/detail/Joint.hh | 9 - test/common_test/joint_features.cc | 256 ++++++++++++++---- 8 files changed, 361 insertions(+), 189 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 6d77d374b..8df941a4c 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -191,10 +191,6 @@ struct JointInfo std::size_t indexInGzModel = 0; btScalar effort = 0; - // True if the fixed constraint's child link is welded to parent link as if - // they are part of the same body. - bool fixedConstraintWeldChildToParent = true; - std::shared_ptr motor = nullptr; std::shared_ptr jointLimits = nullptr; std::shared_ptr fixedConstraint = nullptr; diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 3604f27ca..aa1945246 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -23,6 +23,47 @@ 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) +{ + // 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); +} + ///////////////////////////////////////////////// Identity FreeGroupFeatures::FindFreeGroupForModel( const Identity &_modelID) const @@ -95,8 +136,35 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const auto *model = this->ReferenceInterface(_groupID); if (model) { + btMultiBodyFixedConstraint *fixedConstraintToEnforce = nullptr; + for (auto & joint : this->joints) + { + if (joint.second->fixedConstraint) + { + // If the body is the child of a fixed constraint, do not move update + // its world pose. + btMultiBody *child = joint.second->fixedConstraint->getMultiBodyB(); + if (child == model->body.get()) + { + return; + } + // 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 + btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA(); + if (parent == model->body.get()) + { + fixedConstraintToEnforce = joint.second->fixedConstraint.get(); + } + } + } + model->body->setBaseWorldTransform( convertTf(_pose * model->baseInertiaToLinkFrame.inverse())); + + if (fixedConstraintToEnforce) + enforceFixedConstraint(fixedConstraintToEnforce); + model->body->wakeUp(); } } diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index d9b1730e6..e2003b360 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -27,6 +27,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 +420,7 @@ Identity JointFeatures::AttachFixedJoint( if (world != nullptr && world->world) { world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get()); + jointInfo->fixedConstraint->setMaxAppliedImpulse(btScalar(1e9)); // Make links static if parent or child link is static // This is done by changing collision filter groups / masks @@ -351,18 +431,14 @@ Identity JointFeatures::AttachFixedJoint( { 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) { - btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); - if (childProxy) - { - childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; - childProxy->m_collisionFilterMask = - btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; - } -#if BT_BULLET_VERSION >= 307 - childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); -#endif + makeColliderStatic(linkInfo); + updateColliderFlagsRecursive(std::size_t(_childID), + this->joints, this->links, makeColliderStatic); } } @@ -395,19 +471,15 @@ void JointFeatures::DetachJoint(const Identity &_jointId) btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); if (childProxy) { - // If broadphase and collision object flags do not agree, the - // link was originally non-static but made static by AttachJoint + // Recursively make child colliders dynamic if they were originally + // not static 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 + makeColliderDynamic(linkInfo); + updateColliderFlagsRecursive(std::size_t(jointInfo->childLinkID), + this->joints, this->links, makeColliderDynamic); } } } @@ -441,18 +513,6 @@ void JointFeatures::SetJointTransformFromParent( } } -///////////////////////////////////////////////// -void JointFeatures::SetFixedJointWeldChildToParent( - const Identity &_id, bool _weldChildToParent) -{ - auto jointInfo = this->ReferenceInterface(_id); - - if (jointInfo->fixedConstraint) - { - jointInfo->fixedConstraintWeldChildToParent = _weldChildToParent; - } -} - ///////////////////////////////////////////////// Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( const Identity &_id) const diff --git a/bullet-featherstone/src/JointFeatures.hh b/bullet-featherstone/src/JointFeatures.hh index 42740fbf3..0952d6c44 100644 --- a/bullet-featherstone/src/JointFeatures.hh +++ b/bullet-featherstone/src/JointFeatures.hh @@ -49,9 +49,7 @@ struct JointFeatureList : FeatureList< GetJointTransmittedWrench, - SetMimicConstraintFeature, - - SetFixedJointWeldChildToParentFeature + SetMimicConstraintFeature > { }; class JointFeatures : @@ -140,10 +138,6 @@ class JointFeatures : public: void SetJointTransformFromParent( const Identity &_id, const Pose3d &_pose) override; - // ----- SetFixedJointWeldChildToParentFeature ----- - public: void SetFixedJointWeldChildToParent( - const Identity &_id, bool _weldChildToParent) override; - // ----- Transmitted wrench ----- public: Wrench3d GetJointTransmittedWrenchInJointFrame( const Identity &_id) const override; diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index c85916ad2..a32d93d2c 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -43,57 +43,6 @@ void SimulationFeatures::WorldForwardStep( stepSize = dt.count(); } - // Update fixed constraint behavior if fixedJointWeldChildToParent is true. - // Do this before stepping, i.e. before physics engine tries to solve and - // enforce the constraint - for (auto & joint : this->joints) - { - if (joint.second->fixedConstraint && - joint.second->fixedConstraintWeldChildToParent) - { - // Update fxied constraint's child link pose to maintain a fixed transform - // from the parent link. - btTransform parentToChildTf; - parentToChildTf.setOrigin(joint.second->fixedConstraint->getPivotInA()); - parentToChildTf.setBasis(joint.second->fixedConstraint->getFrameInA()); - btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA(); - btMultiBody *child = joint.second->fixedConstraint->getMultiBodyB(); - - int parentLinkIndex = joint.second->fixedConstraint->getLinkA(); - int childLinkIndex = joint.second->fixedConstraint->getLinkB(); - - btTransform parentLinkTf; - btTransform childLinkTf; - if (parentLinkIndex == -1) - { - parentLinkTf = parent->getBaseWorldTransform(); - } - else - { - btMultiBodyLinkCollider *parentCollider = - parent->getLinkCollider(parentLinkIndex); - parentLinkTf = parentCollider->getWorldTransform(); - } - if (childLinkIndex == -1) - { - childLinkTf = child->getBaseWorldTransform(); - } - else - { - btMultiBodyLinkCollider *childCollider = - child->getLinkCollider(childLinkIndex); - childLinkTf = childCollider->getWorldTransform(); - } - btTransform expectedChildLinkTf = parentLinkTf * parentToChildTf; - btTransform childBaseTf = child->getBaseWorldTransform(); - btTransform childBaseToLink = - childBaseTf.inverse() * childLinkTf; - btTransform newChildBaseTf = - expectedChildLinkTf * childBaseToLink.inverse(); - child->setBaseWorldTransform(newChildBaseTf); - } - } - worldInfo->world->stepSimulation(static_cast(this->stepSize), 1, static_cast(this->stepSize)); diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 85e5005e3..1dfe7dc7f 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -652,38 +652,6 @@ namespace gz Scalar _reference) = 0; }; }; - - ///////////////////////////////////////////////// - class GZ_PHYSICS_VISIBLE SetFixedJointWeldChildToParentFeature - : public virtual Feature - { - /// \brief The Joint API for setting whether to weld a fixed joint's child - /// link to the parent link. - public: template - class Joint : public virtual Feature::Joint - { - /// \brief Set whether to weld the fixed joint's child link to the - /// parent link. If true, the child link is welded and it will move / - /// with the parent link as if they are part of the same body - /// kinematic chain. If false, the fixed joint constraint is enforced - /// by applying forces to both the parent and child links. - /// By default when a fixed joint constraint is created, this property - /// is set to true. - /// \param[in] _weldChildToParent True to weld the child link to the - /// parent link. - public: void SetWeldChildToParent(bool _weldChildToParent); - }; - - /// \private The implementation API for setting whether to weld the fixed - /// joint's child link to the parent link. - public: template - class Implementation : public virtual Feature::Implementation - { - // see Joint::SetWeldChildToParent above - public: virtual void SetFixedJointWeldChildToParent( - const Identity &_id, bool _weldChildToParent) = 0; - }; - }; } } diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index f762b0db1..801042d29 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -263,15 +263,6 @@ namespace gz RelativeWrench(this->GetFrameID(), this->GetTransmittedWrench()), _relativeTo, _inCoordinatesOf); } - - ///////////////////////////////////////////////// - template - void SetFixedJointWeldChildToParentFeature::Joint:: - SetWeldChildToParent(bool _weldChildToParent) - { - this->template Interface() - ->SetFixedJointWeldChildToParent(this->identity, _weldChildToParent); - } } } diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index cba5f1bcc..83f301263 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1297,7 +1297,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) // So here we just check for non-zero velocity #ifdef __APPLE__ // Disable check for dartsim plugin on homebrew. - // jodel3 has zero velocity in dartsim on macOS. It could be a + // 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") @@ -1356,6 +1356,195 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) } } +///////////////////////////////////////////////// +// 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(); + 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 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); + } + } +} + //////////////////////////////////////////////// // Essentially what happens is there are two floating boxes and a box in the // middle that's resting. We start the system out by creating the two @@ -1824,7 +2013,7 @@ TEST_F(WorldModelTest, JointSetCommand) } } -using FixedJointWeldFeatureList = gz::physics::FeatureList< +using FixedJointFreeGroupFeatureList = gz::physics::FeatureList< gz::physics::FindFreeGroupFeature, gz::physics::SetFreeGroupWorldPose, gz::physics::AttachFixedJointFeature, @@ -1838,29 +2027,28 @@ using FixedJointWeldFeatureList = gz::physics::FeatureList< gz::physics::GetModelFromWorld, gz::physics::LinkFrameSemantics, gz::physics::SetBasicJointState, - gz::physics::SetFixedJointWeldChildToParentFeature, gz::physics::SetJointTransformFromParentFeature, gz::physics::SetJointVelocityCommandFeature, gz::physics::sdf::ConstructSdfModel, gz::physics::sdf::ConstructSdfWorld >; -using FixedJointWeldFeatureTestTypes = - JointFeaturesTest; +using FixedJointFreeGroupFeatureTestTypes = + JointFeaturesTest; -TEST_F(FixedJointWeldFeatureTestTypes, FixedJointWeldChildToParent) +TEST_F(FixedJointFreeGroupFeatureTestTypes, FixedJointFreeGroupMove) { - // Attach joint between links from 2 models with different weld child to - // parent property values and verify behavior - // When child is welded to parent, the child behave as if it's in the same - // body as the parent and moves along with the parent link. + // 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); + gz::physics::RequestEngine3d::From( + plugin); ASSERT_NE(nullptr, engine); sdf::Root root; @@ -1914,20 +2102,18 @@ TEST_F(FixedJointWeldFeatureTestTypes, FixedJointWeldChildToParent) EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-2); } - // Attach fixed joint with child welded to parent link - // This should already be true by default + // 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); - fixedJoint->SetWeldChildToParent(true); 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 + // Move parent model using free group freeGroupM1->SetWorldPose(gz::math::eigen3::convert(newModel1Pose)); for (std::size_t i = 0; i < numSteps; ++i) @@ -1942,46 +2128,6 @@ TEST_F(FixedJointWeldFeatureTestTypes, FixedJointWeldChildToParent) gz::math::eigen3::convert(frameDataModel1Body.pose)); EXPECT_EQ(newModel2Pose, gz::math::eigen3::convert(frameDataModel2Body.pose)); - - // Disable weld child to parent link - fixedJoint->SetWeldChildToParent(false); - - // Move the parent model again - newModel1Pose = poseToMoveModel * newModel1Pose; - newModel2Pose = poseToMoveModel * newModel2Pose; - freeGroupM1->SetWorldPose(gz::math::eigen3::convert(newModel1Pose)); - - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); - auto actualModel2Pose = gz::math::eigen3::convert(frameDataModel2Body.pose); - } - frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); - frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); - - // The fixed constraint pulls parent and child model towards each other - // The parent should no longer be at the pose that we set it to be. - auto actualModel1Pose = gz::math::eigen3::convert(frameDataModel1Body.pose); - auto actualModel2Pose = gz::math::eigen3::convert(frameDataModel2Body.pose); - EXPECT_NE(newModel1Pose, actualModel1Pose); - EXPECT_NE(newModel2Pose, actualModel2Pose); - - // Fixed joint constraint should maintain the parent <-> child pose offset - auto expectedModel2Pose = gz::math::eigen3::convert(poseParentChild) * - actualModel1Pose; - // \todo(iche033) Large tol used for bullet-featherstone as it does not - // completely correct position errors. Investigate ways to be more strict in - // enforcing the fixed constraint, e.g. set erp, max applied impulse, etc. - EXPECT_NEAR(expectedModel2Pose.Pos().X(), actualModel2Pose.Pos().X(), 0.2); - EXPECT_NEAR(expectedModel2Pose.Pos().Y(), actualModel2Pose.Pos().Y(), 2e-2); - EXPECT_NEAR(expectedModel2Pose.Pos().Z(), actualModel2Pose.Pos().Z(), 1e-3); - EXPECT_NEAR(expectedModel2Pose.Rot().Euler().X(), - actualModel2Pose.Rot().Euler().X(), 1e-3); - EXPECT_NEAR(expectedModel2Pose.Rot().Euler().Y(), - actualModel2Pose.Rot().Euler().Y(), 1e-3); - EXPECT_NEAR(expectedModel2Pose.Rot().Euler().Z(), - actualModel2Pose.Rot().Euler().Z(), 0.2); } } From 83e31a05d29b28911dee998b55f552dbcbe16767 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 1 Jun 2024 13:31:03 +0000 Subject: [PATCH 19/25] increase tol for bullet version < 3.07 Signed-off-by: Ian Chen --- test/common_test/joint_features.cc | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 83f301263..ab643e4bf 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1436,16 +1436,15 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachChain) // 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. + // Increase tolerance for position. double tol = 1e-3; #ifdef BT_BULLET_VERSION_LE_306 if (this->PhysicsEngineName(name) == "bullet-featherstone") - tol = 1e-2; + tol = 0.1; #endif + EXPECT_NEAR(initialModel2Pose.Pos().Z(), + frameDataModel2Body.pose.translation().z(), tol); EXPECT_NEAR(initialModel3Pose.Pos().Z(), frameDataModel3Body.pose.translation().z(), tol); @@ -1457,13 +1456,13 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachChain) 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. + // So bodies generate non-zero velocities. #ifdef BT_BULLET_VERSION_LE_306 if (this->PhysicsEngineName(name) != "bullet-featherstone") #endif { + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); } } From 12688f9c2150645d6eff18f381bb21c9111f7d24 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 1 Jun 2024 13:34:37 +0000 Subject: [PATCH 20/25] revert one change Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.hh | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/bullet-featherstone/src/JointFeatures.hh b/bullet-featherstone/src/JointFeatures.hh index 0952d6c44..bc6c274f8 100644 --- a/bullet-featherstone/src/JointFeatures.hh +++ b/bullet-featherstone/src/JointFeatures.hh @@ -49,7 +49,9 @@ struct JointFeatureList : FeatureList< GetJointTransmittedWrench, - SetMimicConstraintFeature + SetMimicConstraintFeature, + + FixedJointCast > { }; class JointFeatures : From 97bd03b94e98600f0088350b270d01a7771232f0 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 1 Jun 2024 13:38:21 +0000 Subject: [PATCH 21/25] update comment Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index e2003b360..fceb274c7 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -422,9 +422,8 @@ Identity JointFeatures::AttachFixedJoint( world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get()); jointInfo->fixedConstraint->setMaxAppliedImpulse(btScalar(1e9)); - // Make links static if parent or child link is static - // This is done by changing collision filter groups / masks - // Otherwise bullet will push bodies apart + // 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) @@ -454,8 +453,8 @@ void JointFeatures::DetachJoint(const Identity &_jointId) auto jointInfo = this->ReferenceInterface(_jointId); if (jointInfo->fixedConstraint) { - // Make links dynamic again they were originally not static - // This is done by revert any collision flags / masks changes + // 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()) From 1de0edeffc9f86b725497d70aff2896aa4a62862 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 1 Jun 2024 13:39:06 +0000 Subject: [PATCH 22/25] update comment Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index fceb274c7..9abe50589 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include From c921141177ad0c57a8f3f0515726e10d7315e9da Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 1 Jun 2024 14:41:02 +0000 Subject: [PATCH 23/25] add note about disable collisions between all links and parent and child Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 9abe50589..9618bb20c 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -429,6 +429,10 @@ Identity JointFeatures::AttachFixedJoint( 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); From fce556ecd2380582b00827822035fce336af7a21 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Sun, 2 Jun 2024 08:29:18 -0500 Subject: [PATCH 24/25] Enforce fixed constraints recursively when setting pose on freegroups (#646) Signed-off-by: Addisu Z. Taddese --- bullet-featherstone/src/FreeGroupFeatures.cc | 55 +++++++++++++------- 1 file changed, 36 insertions(+), 19 deletions(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index aa1945246..a77ec9fc3 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -38,7 +38,9 @@ btTransform getWorldTransformForLink(btMultiBody *_body, int _linkIndex) } ///////////////////////////////////////////////// -void enforceFixedConstraint(btMultiBodyFixedConstraint *_fixedConstraint) +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. @@ -62,6 +64,18 @@ void enforceFixedConstraint(btMultiBodyFixedConstraint *_fixedConstraint) 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); + } + } + } } ///////////////////////////////////////////////// @@ -74,6 +88,19 @@ 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; } @@ -136,36 +163,26 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const auto *model = this->ReferenceInterface(_groupID); if (model) { - btMultiBodyFixedConstraint *fixedConstraintToEnforce = nullptr; + model->body->setBaseWorldTransform( + convertTf(_pose * model->baseInertiaToLinkFrame.inverse())); + + model->body->wakeUp(); + for (auto & joint : this->joints) { if (joint.second->fixedConstraint) { - // If the body is the child of a fixed constraint, do not move update - // its world pose. - btMultiBody *child = joint.second->fixedConstraint->getMultiBodyB(); - if (child == model->body.get()) - { - return; - } // 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 + // 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()) { - fixedConstraintToEnforce = joint.second->fixedConstraint.get(); + enforceFixedConstraint(joint.second->fixedConstraint.get(), this->joints); } } } - - model->body->setBaseWorldTransform( - convertTf(_pose * model->baseInertiaToLinkFrame.inverse())); - - if (fixedConstraintToEnforce) - enforceFixedConstraint(fixedConstraintToEnforce); - - model->body->wakeUp(); } } From a11209248420760c30af83136e146a489282f811 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sun, 2 Jun 2024 13:31:34 +0000 Subject: [PATCH 25/25] lint Signed-off-by: Ian Chen --- bullet-featherstone/src/FreeGroupFeatures.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index a77ec9fc3..de0be3dca 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -18,6 +18,7 @@ #include "FreeGroupFeatures.hh" #include +#include namespace gz { namespace physics { @@ -88,7 +89,8 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( if (model->body->hasFixedBase()) return this->GenerateInvalidId(); - // Also reject if the model is a child of a fixed constraint (detachable joint) + // Also reject if the model is a child of a fixed constraint + // (detachable joint) for (const auto & joint : this->joints) { if (joint.second->fixedConstraint) @@ -179,7 +181,8 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA(); if (parent == model->body.get()) { - enforceFixedConstraint(joint.second->fixedConstraint.get(), this->joints); + enforceFixedConstraint(joint.second->fixedConstraint.get(), + this->joints); } } }