From c96ce6ef6a49177ccb38990909fa612a936a4a1c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 22 Apr 2024 16:37:33 -0700 Subject: [PATCH 1/9] bullet-featherstone: Support convex decomposition for meshes (#606) Supports convex decomposition on meshes. Bullet-featherstone implementation will parse the new mesh optimization attribute, decompose the mesh into convex meshes, and builds btConvexHullShape collision shapes. Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 2 +- bullet-featherstone/src/Base.hh | 2 + bullet-featherstone/src/SDFFeatures.cc | 154 ++++++++++++--- dartsim/src/EntityManagement_TEST.cc | 1 + test/common_test/collisions.cc | 247 ++++++++++++++++++++++++- test/include/test/Resources.hh | 1 + test/resources/v_shape.obj | 87 +++++++++ 7 files changed, 464 insertions(+), 30 deletions(-) create mode 100644 test/resources/v_shape.obj diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 66f713223..ac66f5746 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -48,7 +48,7 @@ WorldInfo::WorldInfo(std::string name_) // configuring split impulse and penetration threshold parameters. Instead // the penentration impulse depends on the erp2 parameter so set to a small // value (default in bullet is 0.2). - this->world->getSolverInfo().m_erp2 = btScalar(0.002); + this->world->getSolverInfo().m_erp2 = btScalar(0.02); // Set solver iterations to the same as the default value in SDF, // //world/physics/solver/bullet/iters diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 9ed1d624c..652584f90 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -487,6 +487,7 @@ class Base : public Implements3d> // important. this->meshesGImpact.clear(); this->triangleMeshes.clear(); + this->meshesConvex.clear(); this->joints.clear(); @@ -520,6 +521,7 @@ class Base : public Implements3d> public: std::vector> triangleMeshes; public: std::vector> meshesGImpact; + public: std::vector> meshesConvex; }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 832839b84..0679fb831 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1036,49 +1036,140 @@ bool SDFFeatures::AddSdfCollision( { auto &meshManager = *gz::common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshSdf->Uri()); - const btVector3 scale = convertVec(meshSdf->Scale()); if (nullptr == mesh) { gzwarn << "Failed to load mesh from [" << meshSdf->Uri() << "]." << std::endl; return false; } + const btVector3 scale = convertVec(meshSdf->Scale()); auto compoundShape = std::make_unique(); - for (unsigned int submeshIdx = 0; - submeshIdx < mesh->SubMeshCount(); - ++submeshIdx) + bool meshCreated = false; + if (meshSdf->Optimization() == + ::sdf::MeshOptimization::CONVEX_DECOMPOSITION || + meshSdf->Optimization() == + ::sdf::MeshOptimization::CONVEX_HULL) { - auto s = mesh->SubMeshByIndex(submeshIdx).lock(); - auto vertexCount = s->VertexCount(); - auto indexCount = s->IndexCount(); - btAlignedObjectArray convertedVerts; - convertedVerts.reserve(static_cast(vertexCount)); - for (unsigned int i = 0; i < vertexCount; i++) + std::size_t maxConvexHulls = 16u; + if (meshSdf->Optimization() == ::sdf::MeshOptimization::CONVEX_HULL) + { + /// create 1 convex hull for the whole submesh + maxConvexHulls = 1u; + } + else if (meshSdf->ConvexDecomposition()) { - convertedVerts.push_back(btVector3( - static_cast(s->Vertex(i).X()) * scale[0], - static_cast(s->Vertex(i).Y()) * scale[1], - static_cast(s->Vertex(i).Z()) * scale[2])); + // limit max number of convex hulls to generate + maxConvexHulls = meshSdf->ConvexDecomposition()->MaxConvexHulls(); } - this->triangleMeshes.push_back(std::make_unique()); - for (unsigned int i = 0; i < indexCount/3; i++) + // Check if MeshManager contains the decomposed mesh already. If not + // add it to the MeshManager so we do not need to decompose it again. + const std::string convexMeshName = + mesh->Name() + "_CONVEX_" + std::to_string(maxConvexHulls); + auto *decomposedMesh = meshManager.MeshByName(convexMeshName); + if (!decomposedMesh) { - const btVector3& v0 = convertedVerts[s->Index(i*3)]; - const btVector3& v1 = convertedVerts[s->Index(i*3 + 1)]; - const btVector3& v2 = convertedVerts[s->Index(i*3 + 2)]; - this->triangleMeshes.back()->addTriangle(v0, v1, v2); + // Merge meshes before convex decomposition + auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(*mesh); + if (mergedMesh && mergedMesh->SubMeshCount() == 1u) + { + // Decompose and add mesh to MeshManager + auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock(); + std::vector decomposed = + gz::common::MeshManager::ConvexDecomposition( + *mergedSubmesh.get(), maxConvexHulls); + gzdbg << "Optimizing mesh (" << meshSdf->OptimizationStr() << "): " + << mesh->Name() << std::endl; + // Create decomposed mesh and add it to MeshManager + // Note: MeshManager will call delete on this mesh in its destructor + // \todo(iche033) Consider updating MeshManager to accept + // unique pointers instead + common::Mesh *convexMesh = new common::Mesh; + convexMesh->SetName(convexMeshName); + for (const auto & submesh : decomposed) + convexMesh->AddSubMesh(submesh); + meshManager.AddMesh(convexMesh); + if (decomposed.empty()) + { + // Print an error if convex decomposition returned empty submeshes + // but still add it to MeshManager to avoid going through the + // expensive convex decomposition process for the same mesh again + gzerr << "Convex decomposition generated zero meshes: " + << mesh->Name() << std::endl; + } + decomposedMesh = meshManager.MeshByName(convexMeshName); + } + } + + if (decomposedMesh) + { + for (std::size_t j = 0u; j < decomposedMesh->SubMeshCount(); ++j) + { + auto submesh = decomposedMesh->SubMeshByIndex(j).lock(); + gz::math::Vector3d centroid; + for (std::size_t i = 0; i < submesh->VertexCount(); ++i) + centroid += submesh->Vertex(i); + centroid *= 1.0/static_cast(submesh->VertexCount()); + btAlignedObjectArray vertices; + for (std::size_t i = 0; i < submesh->VertexCount(); ++i) + { + gz::math::Vector3d v = submesh->Vertex(i) - centroid; + vertices.push_back(convertVec(v) * scale); + } + + float collisionMargin = 0.001f; + this->meshesConvex.push_back(std::make_unique( + &(vertices[0].getX()), vertices.size())); + auto *convexShape = this->meshesConvex.back().get(); + convexShape->setMargin(collisionMargin); + + btTransform trans; + trans.setIdentity(); + trans.setOrigin(convertVec(centroid) * scale); + compoundShape->addChildShape(trans, convexShape); + } + meshCreated = true; } + } + + if (!meshCreated) + { + for (unsigned int submeshIdx = 0; + submeshIdx < mesh->SubMeshCount(); + ++submeshIdx) + { + auto s = mesh->SubMeshByIndex(submeshIdx).lock(); + auto vertexCount = s->VertexCount(); + auto indexCount = s->IndexCount(); + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(static_cast(vertexCount)); + for (unsigned int i = 0; i < vertexCount; i++) + { + convertedVerts.push_back(btVector3( + static_cast(s->Vertex(i).X()) * scale[0], + static_cast(s->Vertex(i).Y()) * scale[1], + static_cast(s->Vertex(i).Z()) * scale[2])); + } - this->meshesGImpact.push_back( - std::make_unique( - this->triangleMeshes.back().get())); - this->meshesGImpact.back()->updateBound(); - this->meshesGImpact.back()->setMargin(btScalar(0.01)); - compoundShape->addChildShape(btTransform::getIdentity(), - this->meshesGImpact.back().get()); + this->triangleMeshes.push_back(std::make_unique()); + for (unsigned int i = 0; i < indexCount/3; i++) + { + const btVector3& v0 = convertedVerts[s->Index(i*3)]; + const btVector3& v1 = convertedVerts[s->Index(i*3 + 1)]; + const btVector3& v2 = convertedVerts[s->Index(i*3 + 2)]; + this->triangleMeshes.back()->addTriangle(v0, v1, v2); + } + + this->meshesGImpact.push_back( + std::make_unique( + this->triangleMeshes.back().get())); + this->meshesGImpact.back()->updateBound(); + this->meshesGImpact.back()->setMargin(btScalar(0.01)); + compoundShape->addChildShape(btTransform::getIdentity(), + this->meshesGImpact.back().get()); + } } shape = std::move(compoundShape); } @@ -1186,6 +1277,15 @@ bool SDFFeatures::AddSdfCollision( btVector3(static_cast(mu), static_cast(mu2), 1), btCollisionObject::CF_ANISOTROPIC_FRICTION); + if (geom->MeshShape()) + { + // Set meshes to use softer contacts for stability + // \todo(iche033) load and values from SDF + const btScalar kp = btScalar(1e15); + const btScalar kd = btScalar(1e14); + linkInfo->collider->setContactStiffnessAndDamping(kp, kd); + } + if (linkIndexInModel >= 0) { model->body->getLink(linkIndexInModel).m_collider = diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 52a3e9b4a..b885a73f0 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -177,6 +177,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) const std::string meshFilename = gz::physics::test::resources::kChassisDae; auto &meshManager = *common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); + ASSERT_NE(nullptr, mesh); auto meshShape = meshLink->AttachMeshShape("chassis", *mesh); const auto originalMeshSize = mesh->Max() - mesh->Min(); diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index db2567e41..647bffa39 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -18,8 +18,11 @@ #include #include +#include #include +#include +#include #include #include "test/Resources.hh" @@ -40,8 +43,6 @@ #include #include -#include - #include template @@ -110,6 +111,7 @@ TYPED_TEST(CollisionTest, MeshAndPlane) const std::string meshFilename = gz::physics::test::resources::kChassisDae; auto &meshManager = *gz::common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); + ASSERT_NE(nullptr, mesh); // TODO(anyone): This test is somewhat awkward because we lift up the mesh // from the center of the link instead of lifting up the link or the model. @@ -147,6 +149,247 @@ TYPED_TEST(CollisionTest, MeshAndPlane) } } +using CollisionMeshFeaturesList = gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::LinkFrameSemantics, + gz::physics::ForwardStep, + gz::physics::GetEntities +>; + +using CollisionMeshTestFeaturesList = CollisionTest; + +TEST_F(CollisionMeshTestFeaturesList, MeshOptimization) +{ + // Load an optimized mesh, drop it from some height, + // and verify it collides with the ground plane + + auto getModelOptimizedStr = [](const std::string &_optimization, + const std::string &_name, + const gz::math::Pose3d &_pose) + { + std::stringstream modelOptimizedStr; + modelOptimizedStr << R"( + + + )"; + modelOptimizedStr << _pose; + modelOptimizedStr << R"( + + + + + )"; + modelOptimizedStr << gz::physics::test::resources::kChassisDae; + modelOptimizedStr << R"( + + + + + + )"; + return modelOptimizedStr.str(); + }; + + for (const std::string &name : this->pluginNames) + { + // currently only bullet-featherstone supports mesh decomposition + if (this->PhysicsEngineName(name) != "bullet-featherstone") + continue; + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + sdf::Root rootWorld; + const sdf::Errors errorsWorld = + rootWorld.Load(common_test::worlds::kGroundSdf); + ASSERT_TRUE(errorsWorld.empty()) << errorsWorld.front(); + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + auto world = engine->ConstructWorld(*rootWorld.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + // load the mesh into mesh manager first to create a cache + // so the model can be constructed later - needed by bullet-featherstone + const std::string meshFilename = gz::physics::test::resources::kChassisDae; + auto &meshManager = *gz::common::MeshManager::Instance(); + ASSERT_NE(nullptr, meshManager.Load(meshFilename)); + + std::unordered_set optimizations; + optimizations.insert(""); + optimizations.insert("convex_decomposition"); + optimizations.insert("convex_hull"); + + gz::math::Pose3d initialModelPose(0, 0, 2, 0, 0, 0); + // Test all optimization methods + for (const auto &optimizationStr : optimizations) + { + // create the chassis model + const std::string modelOptimizedName = "model_optimized_" + optimizationStr; + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(getModelOptimizedStr( + optimizationStr, modelOptimizedName, initialModelPose)); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + const std::string bodyName{"body"}; + auto modelOptimized = world->GetModel(modelOptimizedName); + auto modelOptimizedBody = modelOptimized->GetLink(bodyName); + + auto frameDataModelOptimizedBody = + modelOptimizedBody->FrameDataRelativeToWorld(); + + EXPECT_EQ(initialModelPose, + gz::math::eigen3::convert(frameDataModelOptimizedBody.pose)); + + // After a while, the mesh model should reach the ground and come to a stop + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + std::size_t stepCount = 3000u; + for (unsigned int i = 0; i < stepCount; ++i) + world->Step(output, state, input); + + frameDataModelOptimizedBody = + modelOptimizedBody->FrameDataRelativeToWorld(); + + // convex decomposition gives more accurate results + double tol = (optimizationStr == "convex_decomposition") ? 1e-3 : 1e-2; + EXPECT_NEAR(0.1, + frameDataModelOptimizedBody.pose.translation().z(), tol); + EXPECT_NEAR(0.0, frameDataModelOptimizedBody.linearVelocity.z(), tol); + + initialModelPose.Pos() += gz::math::Vector3d(0, 2, 0); + } + } +} + +TEST_F(CollisionMeshTestFeaturesList, MeshDecomposition) +{ + // Load a convex decomposed V shape mesh, drop a sphere over it + // and verify the sphere falls inside of the V shape groove and rests + // on top of it + + std::string modelStr = R"( + + + 0 0 0.0 0 0 0 + + + + + + 64 + + )"; + modelStr += gz::physics::test::resources::kVShapeObj; + modelStr += R"( + + + + + + )"; + + const std::string sphereStr = R"( + + + 0 0 1.0 0 0 0 + + + + + 0.1 + + + + + + )"; + + for (const std::string &name : this->pluginNames) + { + // currently only bullet-featherstone supports mesh decomposition + if (this->PhysicsEngineName(name) != "bullet-featherstone") + continue; + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + sdf::Root rootWorld; + const sdf::Errors errorsWorld = + rootWorld.Load(common_test::worlds::kGroundSdf); + ASSERT_TRUE(errorsWorld.empty()) << errorsWorld; + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + auto world = engine->ConstructWorld(*rootWorld.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + // load the mesh into mesh manager first to create a cache + // so the model can be constructed later - needed by bullet-featherstone + const std::string meshFilename = gz::physics::test::resources::kVShapeObj; + auto &meshManager = *gz::common::MeshManager::Instance(); + ASSERT_NE(nullptr, meshManager.Load(meshFilename)); + + // create the v shape model + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(modelStr); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + auto model = world->GetModel("v_shape"); + auto link = model->GetLink("link"); + auto frameDataLink = link->FrameDataRelativeToWorld(); + EXPECT_EQ(gz::math::Pose3d::Zero, + gz::math::eigen3::convert(frameDataLink.pose)); + + // spawn a sphere over the v shape model + errors = root.LoadSdfString(sphereStr); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + auto sphere = world->GetModel("sphere"); + auto sphereLink = sphere->GetLink("link"); + auto frameDataSphereLink = sphereLink->FrameDataRelativeToWorld(); + EXPECT_EQ( + gz::math::Pose3d(0, 0, 1, 0, 0, 0), + gz::math::eigen3::convert(frameDataSphereLink.pose)); + + // After a while, the sphere model should drop inside the V shape model + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + std::size_t stepCount = 3000u; + for (unsigned int i = 0; i < stepCount; ++i) + world->Step(output, state, input); + + frameDataLink = link->FrameDataRelativeToWorld(); + frameDataSphereLink = sphereLink->FrameDataRelativeToWorld(); + + // V shape mesh should be at the same pose + EXPECT_NEAR(0.0, frameDataLink.pose.translation().x(), 1e-3); + EXPECT_NEAR(0.0, frameDataLink.pose.translation().y(), 1e-3); + EXPECT_NEAR(0.0, frameDataLink.pose.translation().z(), 1e-2); + + // sphere should rest inside the of V shape + EXPECT_NEAR(0.0, frameDataSphereLink.pose.translation().x(), 1e-3); + EXPECT_NEAR(0.0, frameDataSphereLink.pose.translation().y(), 1e-2); + EXPECT_NEAR(0.523, frameDataSphereLink.pose.translation().z(), 1e-2); + EXPECT_NEAR(0.0, frameDataSphereLink.linearVelocity.x(), 1e-3); + EXPECT_NEAR(0.0, frameDataSphereLink.linearVelocity.y(), 1e-3); + EXPECT_NEAR(0.0, frameDataSphereLink.linearVelocity.z(), 1e-3); + } +} + using CollisionStaticFeaturesList = gz::physics::FeatureList< gz::physics::sdf::ConstructSdfModel, gz::physics::sdf::ConstructSdfWorld, diff --git a/test/include/test/Resources.hh b/test/include/test/Resources.hh index 8b2eff9c7..398580667 100644 --- a/test/include/test/Resources.hh +++ b/test/include/test/Resources.hh @@ -33,6 +33,7 @@ const auto kChassisDae = TestResource("chassis.dae"); const auto kHeightmapBowlPng = TestResource("heightmap_bowl.png"); const auto kRrbotXml = TestResource("rrbot.xml"); const auto kVolcanoTif = TestResource("volcano.tif"); +const auto kVShapeObj = TestResource("v_shape.obj"); } // namespace gz::physics::test #endif // TEST_RESOURCES_HH_ diff --git a/test/resources/v_shape.obj b/test/resources/v_shape.obj new file mode 100644 index 000000000..fd1afe0bb --- /dev/null +++ b/test/resources/v_shape.obj @@ -0,0 +1,87 @@ +# Blender 4.0.2 +# www.blender.org +o V_shape +v 0.080247 -0.297536 0.711850 +v 0.080247 -0.200536 0.711850 +v 0.080247 0.003464 0.232850 +v 0.080247 0.208464 0.711850 +v 0.080247 0.306464 0.711850 +v 0.080247 0.011464 0.023850 +v 0.080247 -0.008536 0.023850 +v -0.083133 -0.297536 0.711850 +v -0.083133 -0.200536 0.711850 +v -0.083133 0.003464 0.232850 +v -0.083133 0.208464 0.711850 +v -0.083133 0.306464 0.711850 +v -0.083133 0.011464 0.023850 +v -0.083133 -0.008536 0.023850 +vn 1.0000 -0.0000 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 0.9200 0.3918 +vn -0.0000 -0.9220 -0.3873 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 0.9191 -0.3941 +vn -0.0000 -0.9193 0.3935 +vt 0.833333 0.000000 +vt 0.666667 0.000000 +vt 0.500000 0.000000 +vt 0.333333 0.000000 +vt 0.166667 0.000000 +vt 0.000000 0.000000 +vt 1.000000 0.000000 +s 0 +usemtl Material +f 6/1/1 5/2/1 4/3/1 +f 6/1/1 4/3/1 3/4/1 +f 3/4/1 2/5/1 1/6/1 +f 3/4/1 1/6/1 7/7/1 +f 6/1/1 3/4/1 7/7/1 +f 13/1/2 11/3/2 12/2/2 +f 13/1/2 10/4/2 11/3/2 +f 10/4/2 8/6/2 9/5/2 +f 10/4/2 14/7/2 8/6/2 +f 13/1/2 14/7/2 10/4/2 +f 1/6/3 2/5/3 9/5/3 8/6/3 +f 4/3/3 5/2/3 12/2/3 11/3/3 +f 2/5/4 3/4/4 10/4/4 9/5/4 +f 7/7/5 1/6/5 8/6/5 14/7/5 +f 6/1/6 7/7/6 14/7/6 13/1/6 +f 5/2/7 6/1/7 13/1/7 12/2/7 +f 3/4/8 4/3/8 11/3/8 10/4/8 +o base +v -0.500000 0.500000 0.003279 +v -0.500000 0.500000 0.023279 +v 0.500000 0.500000 0.003279 +v 0.500000 0.500000 0.023279 +v -0.500000 -0.500000 0.003279 +v -0.500000 -0.500000 0.023279 +v 0.500000 -0.500000 0.003279 +v 0.500000 -0.500000 0.023279 +vn -0.0000 1.0000 -0.0000 +vn 1.0000 -0.0000 -0.0000 +vn -0.0000 -1.0000 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 -0.0000 1.0000 +vt 0.375000 0.000000 +vt 0.625000 0.000000 +vt 0.625000 0.250000 +vt 0.375000 0.250000 +vt 0.625000 0.500000 +vt 0.375000 0.500000 +vt 0.625000 0.750000 +vt 0.375000 0.750000 +vt 0.625000 1.000000 +vt 0.375000 1.000000 +vt 0.125000 0.500000 +vt 0.125000 0.750000 +vt 0.875000 0.500000 +vt 0.875000 0.750000 +s 0 +f 15/8/9 16/9/9 18/10/9 17/11/9 +f 17/11/10 18/10/10 22/12/10 21/13/10 +f 21/13/11 22/12/11 20/14/11 19/15/11 +f 19/15/12 20/14/12 16/16/12 15/17/12 +f 17/18/13 21/13/13 19/15/13 15/19/13 +f 22/12/14 18/20/14 16/21/14 20/14/14 From 62ee52cf00c39b50eeea3570f79623afa2225fc8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 23 Apr 2024 19:57:12 -0700 Subject: [PATCH 2/9] bullet-featurestore: Enable auto deactivation (#630) Signed-off-by: Ian Chen --- bullet-featherstone/src/FreeGroupFeatures.cc | 3 +++ bullet-featherstone/src/JointFeatures.cc | 6 +++++- bullet-featherstone/src/LinkFeatures.cc | 2 ++ bullet-featherstone/src/SimulationFeatures.cc | 19 ------------------- 4 files changed, 10 insertions(+), 20 deletions(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 4fd79f57f..3604f27ca 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -69,6 +69,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( if (model) { model->body->setBaseOmega(convertVec(_angularVelocity)); + model->body->wakeUp(); } } @@ -82,6 +83,7 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( if (model) { model->body->setBaseVel(convertVec(_linearVelocity)); + model->body->wakeUp(); } } @@ -95,6 +97,7 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( { model->body->setBaseWorldTransform( convertTf(_pose * model->baseInertiaToLinkFrame.inverse())); + model->body->wakeUp(); } } diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 980313a7b..588a26a6a 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -112,6 +112,7 @@ void JointFeatures::SetJointPosition( const auto *model = this->ReferenceInterface(joint->model); model->body->getJointPosMultiDof(identifier->indexInBtModel)[_dof] = static_cast(_value); + model->body->wakeUp(); } ///////////////////////////////////////////////// @@ -126,6 +127,7 @@ void JointFeatures::SetJointVelocity( const auto *model = this->ReferenceInterface(joint->model); model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] = static_cast(_value); + model->body->wakeUp(); } ///////////////////////////////////////////////// @@ -156,6 +158,7 @@ void JointFeatures::SetJointForce( const auto *model = this->ReferenceInterface(joint->model); model->body->getJointTorqueMultiDof( identifier->indexInBtModel)[_dof] = static_cast(_value); + model->body->wakeUp(); } ///////////////////////////////////////////////// @@ -282,9 +285,9 @@ void JointFeatures::SetJointVelocityCommand( return; } + auto modelInfo = this->ReferenceInterface(jointInfo->model); if (!jointInfo->motor) { - auto modelInfo = this->ReferenceInterface(jointInfo->model); jointInfo->motor = std::make_shared( modelInfo->body.get(), std::get(jointInfo->identifier).indexInBtModel, @@ -296,6 +299,7 @@ void JointFeatures::SetJointVelocityCommand( } jointInfo->motor->setVelocityTarget(static_cast(_value)); + modelInfo->body->wakeUp(); } ///////////////////////////////////////////////// diff --git a/bullet-featherstone/src/LinkFeatures.cc b/bullet-featherstone/src/LinkFeatures.cc index b8dc44356..29e227d7d 100644 --- a/bullet-featherstone/src/LinkFeatures.cc +++ b/bullet-featherstone/src/LinkFeatures.cc @@ -50,6 +50,7 @@ void LinkFeatures::AddLinkExternalForceInWorld( model->body->addBaseForce(F); model->body->addBaseTorque(relPosWorld.cross(F)); } + model->body->wakeUp(); } ///////////////////////////////////////////////// @@ -73,6 +74,7 @@ void LinkFeatures::AddLinkExternalTorqueInWorld( btVector3 torqueWorld = model->body->getBaseWorldTransform().getBasis() * T; model->body->addBaseTorque(torqueWorld); } + model->body->wakeUp(); } } diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index a34a6741c..a32d93d2c 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -46,25 +46,6 @@ void SimulationFeatures::WorldForwardStep( worldInfo->world->stepSimulation(static_cast(this->stepSize), 1, static_cast(this->stepSize)); - for (auto & m : this->models) - { - if (m.second->body) - { - m.second->body->checkMotionAndSleepIfRequired( - static_cast(this->stepSize)); - btMultiBodyLinkCollider* col = m.second->body->getBaseCollider(); - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState(ACTIVE_TAG); - - for (int b = 0; b < m.second->body->getNumLinks(); b++) - { - col = m.second->body->getLink(b).m_collider; - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState(ACTIVE_TAG); - } - } - } - this->WriteRequiredData(_h); this->Write(_h.Get()); } From ca40aee86ca8fbfb9a1067fab358ca8ac1dbf370 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 3 May 2024 15:59:30 -0500 Subject: [PATCH 3/9] Add package.xml (#608) Signed-off-by: Addisu Z. Taddese --- .github/workflows/package_xml.yml | 11 +++++++++++ package.xml | 27 +++++++++++++++++++++++++++ 2 files changed, 38 insertions(+) create mode 100644 .github/workflows/package_xml.yml create mode 100644 package.xml diff --git a/.github/workflows/package_xml.yml b/.github/workflows/package_xml.yml new file mode 100644 index 000000000..4bd4a9aa0 --- /dev/null +++ b/.github/workflows/package_xml.yml @@ -0,0 +1,11 @@ +name: Validate package.xml + +on: + pull_request: + +jobs: + package-xml: + runs-on: ubuntu-latest + name: Validate package.xml + steps: + - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy diff --git a/package.xml b/package.xml new file mode 100644 index 000000000..964ebee39 --- /dev/null +++ b/package.xml @@ -0,0 +1,27 @@ + + + gz-physics7 + 7.2.0 + Gazebo Physics : Physics classes and functions for robot applications + Steve Peters + Apache License 2.0 + https://github.com/gazebosim/gz-physics + + cmake + + gz-cmake3 + + benchmark + bullet + DART + eigen + gz-common5 + gz-math7 + gz-plugin2 + gz-utils2 + sdformat14 + + + cmake + + From 0f59151bb2a6416c999519d0ed46e3e29e39e913 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 14 May 2024 02:42:10 +0900 Subject: [PATCH 4/9] bullet-featherstone: Fix convex hull shape's AABB (#637) Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 0679fb831..7a0e444b3 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1124,6 +1124,8 @@ bool SDFFeatures::AddSdfCollision( &(vertices[0].getX()), vertices.size())); auto *convexShape = this->meshesConvex.back().get(); convexShape->setMargin(collisionMargin); + convexShape->recalcLocalAabb(); + convexShape->optimizeConvexHull(); btTransform trans; trans.setIdentity(); From a74dae4b732e9c7932a037bc931956956ba4f6ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=98ystein=20Sture?= Date: Mon, 27 May 2024 05:47:24 -0300 Subject: [PATCH 5/9] Update InspectFeatures.hh (#645) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Øystein Sture --- include/gz/physics/detail/InspectFeatures.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/include/gz/physics/detail/InspectFeatures.hh b/include/gz/physics/detail/InspectFeatures.hh index 4a02210f2..ca58e6d4c 100644 --- a/include/gz/physics/detail/InspectFeatures.hh +++ b/include/gz/physics/detail/InspectFeatures.hh @@ -18,6 +18,7 @@ #ifndef GZ_PHYSICS_DETAIL_INSPECTFEATURES_HH #define GZ_PHYSICS_DETAIL_INSPECTFEATURES_HH +#include #include #include #include From 74c4560e7b263e46b61dacff7ee30fff9efd5a19 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 3 Jun 2024 21:31:12 +0900 Subject: [PATCH 6/9] bullet-featherstone: Update fixed constraint behavior (#632) * bullet-featherstone fixed joint implementation now calls setMaxAppliedImpulse to enforce fixed constraint. *When the pose of the parent body of a fixed constraint is changed by FreeGroupSetWorldPose, the child pose is updated to enforce pose transform relative to the parent * prevent collision between parent and child link if parent is static Signed-off-by: Ian Chen Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- bullet-featherstone/src/Base.hh | 22 +- bullet-featherstone/src/FreeGroupFeatures.cc | 88 +++ bullet-featherstone/src/JointFeatures.cc | 137 +++++ bullet-featherstone/src/SDFFeatures.cc | 8 +- test/common_test/CMakeLists.txt | 5 + test/common_test/Worlds.hh | 2 + test/common_test/joint_features.cc | 562 ++++++++++++++++-- .../worlds/joint_across_models_fixed.sdf | 145 +++++ 8 files changed, 932 insertions(+), 37 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..8df941a4c 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -112,6 +112,24 @@ struct ModelInfo } }; +/// \brief Custom GzMultiBodyLinkCollider class +class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider { + using btMultiBodyLinkCollider::btMultiBodyLinkCollider; + + /// \brief Overrides base function to enable support for ignoring + /// collision with objects from other bodies if + /// btCollisionObject::setIgnoreCollisionCheck is called. + /// Note: originally btMultiBodyLinkCollider::checkCollideWithOverride + /// just returns true if the input collision object is from a + /// different body and disregards any setIgnoreCollisionCheck calls. + public: bool checkCollideWithOverride(const btCollisionObject *_co) const + override + { + return btMultiBodyLinkCollider::checkCollideWithOverride(_co) && + btCollisionObject::checkCollideWithOverride(_co); + } +}; + /// Link information is embedded inside the model, so all we need to store here /// is a reference to the model and the index of this link inside of it. struct LinkInfo @@ -120,10 +138,12 @@ struct LinkInfo std::optional indexInModel; Identity model; Eigen::Isometry3d inertiaToLinkFrame; - std::unique_ptr collider = nullptr; + std::unique_ptr collider = nullptr; std::unique_ptr shape = nullptr; std::vector collisionEntityIds = {}; std::unordered_map collisionNameToEntityId = {}; + // Link is either static, fixed to world, or has zero dofs + bool isStaticOrFixed = false; }; struct CollisionInfo diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 3604f27ca..de0be3dca 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -18,11 +18,67 @@ #include "FreeGroupFeatures.hh" #include +#include namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// +btTransform getWorldTransformForLink(btMultiBody *_body, int _linkIndex) +{ + if (_linkIndex == -1) + { + return _body->getBaseWorldTransform(); + } + else + { + btMultiBodyLinkCollider *collider = _body->getLinkCollider(_linkIndex); + return collider->getWorldTransform(); + } +} + +///////////////////////////////////////////////// +void enforceFixedConstraint( + btMultiBodyFixedConstraint *_fixedConstraint, + const std::unordered_map &_allJoints) +{ + // Update fixed constraint's child link pose to maintain a fixed transform + // from the parent link. + btMultiBody *parent = _fixedConstraint->getMultiBodyA(); + btMultiBody *child = _fixedConstraint->getMultiBodyB(); + + btTransform parentToChildTf; + parentToChildTf.setOrigin(_fixedConstraint->getPivotInA()); + parentToChildTf.setBasis(_fixedConstraint->getFrameInA()); + + int parentLinkIndex = _fixedConstraint->getLinkA(); + int childLinkIndex = _fixedConstraint->getLinkB(); + + btTransform parentLinkTf = getWorldTransformForLink(parent, parentLinkIndex); + btTransform childLinkTf = getWorldTransformForLink(child, childLinkIndex); + + btTransform expectedChildLinkTf = parentLinkTf * parentToChildTf; + btTransform childBaseTf = child->getBaseWorldTransform(); + btTransform childBaseToLink = + childBaseTf.inverse() * childLinkTf; + btTransform newChildBaseTf = + expectedChildLinkTf * childBaseToLink.inverse(); + child->setBaseWorldTransform(newChildBaseTf); + for (const auto &joint : _allJoints) + { + if (joint.second->fixedConstraint) + { + // Recursively enforce constraints where the child here is a parent to + // other constraints. + if (joint.second->fixedConstraint->getMultiBodyA() == child) + { + enforceFixedConstraint(joint.second->fixedConstraint.get(), _allJoints); + } + } + } +} + ///////////////////////////////////////////////// Identity FreeGroupFeatures::FindFreeGroupForModel( const Identity &_modelID) const @@ -33,6 +89,20 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( if (model->body->hasFixedBase()) return this->GenerateInvalidId(); + // Also reject if the model is a child of a fixed constraint + // (detachable joint) + for (const auto & joint : this->joints) + { + if (joint.second->fixedConstraint) + { + if (joint.second->fixedConstraint->getMultiBodyB() == model->body.get()) + { + return this->GenerateInvalidId(); + } + } + } + + return _modelID; } @@ -97,7 +167,25 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( { model->body->setBaseWorldTransform( convertTf(_pose * model->baseInertiaToLinkFrame.inverse())); + model->body->wakeUp(); + + for (auto & joint : this->joints) + { + if (joint.second->fixedConstraint) + { + // Only the parent body of a fixed joint can be moved but we need to + // enforce the fixed constraint by updating the child pose so it + // remains at a fixed transform from parent. We do this recursively to + // account for other constraints attached to the child. + btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA(); + if (parent == model->body.get()) + { + enforceFixedConstraint(joint.second->fixedConstraint.get(), + this->joints); + } + } + } } } diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 588a26a6a..9618bb20c 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -27,6 +28,85 @@ namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// +void makeColliderStatic(LinkInfo *_linkInfo) +{ + btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get(); + if (!childCollider) + return; + + // if link is already static or fixed, we do not need to change its + // collision flags + if (_linkInfo->isStaticOrFixed) + return; + + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (!childProxy) + return; + + childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; + childProxy->m_collisionFilterMask = + btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; +#if BT_BULLET_VERSION >= 307 + childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); +#endif +} + +///////////////////////////////////////////////// +void makeColliderDynamic(LinkInfo *_linkInfo) +{ + btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get(); + if (!childCollider) + return; + + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (!childProxy) + return; + + // If broadphase and collision object flags do not agree, the + // link was originally non-static but made static by AttachJoint + if (!_linkInfo->isStaticOrFixed && + ((childProxy->m_collisionFilterGroup & + btBroadphaseProxy::StaticFilter) > 0)) + { + childProxy->m_collisionFilterGroup = + btBroadphaseProxy::DefaultFilter; + childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter; +#if BT_BULLET_VERSION >= 307 + childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); +#endif + } +} + +///////////////////////////////////////////////// +void updateColliderFlagsRecursive(std::size_t _linkID, + const std::unordered_map> &_joints, + const std::unordered_map> &_links, + std::function _updateColliderCb) +{ + btMultiBodyFixedConstraint *fixedConstraint = nullptr; + std::size_t childLinkID = 0u; + for (const auto &joint : _joints) + { + if (!joint.second->fixedConstraint) + continue; + if (!joint.second->parentLinkID.has_value() || + joint.second->parentLinkID.value() != _linkID) + continue; + + fixedConstraint = joint.second->fixedConstraint.get(); + childLinkID = std::size_t(joint.second->childLinkID); + } + + if (!fixedConstraint) + return; + + auto childInfo = _links.at(childLinkID); + _updateColliderCb(childInfo.get()); + + updateColliderFlagsRecursive(childLinkID, _joints, _links, _updateColliderCb); +} + ///////////////////////////////////////////////// double JointFeatures::GetJointPosition( const Identity &_id, const std::size_t _dof) const @@ -341,6 +421,31 @@ Identity JointFeatures::AttachFixedJoint( if (world != nullptr && world->world) { world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get()); + jointInfo->fixedConstraint->setMaxAppliedImpulse(btScalar(1e9)); + + // Make child link static if parent is static + // This is done by updating collision flags + btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get(); + btMultiBodyLinkCollider *childCollider = linkInfo->collider.get(); + if (parentCollider && childCollider) + { + // disable collision between parent and child collider + // \todo(iche033) if self collide is true, extend this to + // disable collisions between all the links in the parent's model with + // all the links in the child's model. + parentCollider->setIgnoreCollisionCheck(childCollider, true); + childCollider->setIgnoreCollisionCheck(parentCollider, true); + + // If parent link is static or fixed, recusively update child colliders + // collision flags to be static. + if (parentLinkInfo->isStaticOrFixed && !linkInfo->isStaticOrFixed) + { + makeColliderStatic(linkInfo); + updateColliderFlagsRecursive(std::size_t(_childID), + this->joints, this->links, makeColliderStatic); + } + } + return this->GenerateIdentity(jointID, this->joints.at(jointID)); } @@ -353,6 +458,37 @@ void JointFeatures::DetachJoint(const Identity &_jointId) auto jointInfo = this->ReferenceInterface(_jointId); if (jointInfo->fixedConstraint) { + // Make links dynamic again as they were originally not static + // This is done by reverting any collision flag changes + // made in AttachJoint + auto *linkInfo = this->ReferenceInterface(jointInfo->childLinkID); + if (jointInfo->parentLinkID.has_value()) + { + auto parentLinkInfo = this->links.at(jointInfo->parentLinkID.value()); + + btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get(); + btMultiBodyLinkCollider *childCollider = linkInfo->collider.get(); + if (parentCollider && childCollider) + { + parentCollider->setIgnoreCollisionCheck(childCollider, false); + childCollider->setIgnoreCollisionCheck(parentCollider, false); + btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle(); + if (childProxy) + { + // Recursively make child colliders dynamic if they were originally + // not static + if (!linkInfo->isStaticOrFixed && + ((childProxy->m_collisionFilterGroup & + btBroadphaseProxy::StaticFilter) > 0)) + { + makeColliderDynamic(linkInfo); + updateColliderFlagsRecursive(std::size_t(jointInfo->childLinkID), + this->joints, this->links, makeColliderDynamic); + } + } + } + } + auto modelInfo = this->ReferenceInterface(jointInfo->model); if (modelInfo) { @@ -360,6 +496,7 @@ void JointFeatures::DetachJoint(const Identity &_jointId) world->world->removeMultiBodyConstraint(jointInfo->fixedConstraint.get()); jointInfo->fixedConstraint.reset(); jointInfo->fixedConstraint = nullptr; + modelInfo->body->wakeUp(); } } } diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 7a0e444b3..15e7125a5 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1263,7 +1263,7 @@ bool SDFFeatures::AddSdfCollision( // NOTE: Bullet does not appear to support different surface properties // for different shapes attached to the same link. - linkInfo->collider = std::make_unique( + linkInfo->collider = std::make_unique( model->body.get(), linkIndexInModel); linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); @@ -1330,6 +1330,12 @@ bool SDFFeatures::AddSdfCollision( linkInfo->collider.get(), btBroadphaseProxy::StaticFilter, btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + linkInfo->isStaticOrFixed = true; + + // Set collider collision flags +#if BT_BULLET_VERSION >= 307 + linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT); +#endif } else { diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 4df041547..eac823f60 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -76,6 +76,11 @@ foreach(test ${tests}) "BT_BULLET_VERSION_LE_325" ) endif() + if (bullet_version_check_VERSION VERSION_LESS_EQUAL 3.06) + target_compile_definitions(${test_executable} PRIVATE + "BT_BULLET_VERSION_LE_306" + ) + endif() if (DART_HAS_CONTACT_SURFACE_HEADER) target_compile_definitions(${test_executable} PRIVATE DART_HAS_CONTACT_SURFACE) diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index 7ac4284d5..2328cb033 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -36,6 +36,8 @@ const auto kFallingWorld = CommonTestWorld("falling.world"); const auto kFallingAddedMassWorld = CommonTestWorld("falling_added_mass.world"); const auto kGroundSdf = CommonTestWorld("ground.sdf"); const auto kJointAcrossModelsSdf = CommonTestWorld("joint_across_models.sdf"); +const auto kJointAcrossModelsFixedSdf = + CommonTestWorld("joint_across_models_fixed.sdf"); const auto kJointConstraintSdf = CommonTestWorld("joint_constraint.sdf"); const auto kMimicFastSlowPendulumsWorld = CommonTestWorld("mimic_fast_slow_pendulums_world.sdf"); diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index b19f0e980..ab643e4bf 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -14,6 +14,10 @@ * limitations under the License. * */ + +#include +#include +#include #include #include @@ -22,6 +26,7 @@ #include #include +#include #include #include "test/TestLibLoader.hh" @@ -1146,22 +1151,396 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) } // After a while, body2 should reach the ground and come to a stop - std::size_t stepCount = 0u; - const std::size_t maxNumSteps = 2000u; - while (stepCount++ < maxNumSteps) + for (std::size_t i = 0; i < 1000; ++i) + { + world->Step(output, state, input); + } + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + } +} + +///////////////////////////////////////////////// +// Attach a fixed joint between links from non-static models to a link +// from a model fixed to the world. Verify the models are not moving when +// the fixed joint is attached. +TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsFixedSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // M1 is fixed to the world + // M2 is non-static and at some distance away from M1 + // M3 is non-static and overlaps with M1 + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string modelName3{"M3"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model3 = world->GetModel(modelName3); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + auto model3Body = model3->GetLink(bodyName); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + auto frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + const gz::math::Pose3d initialModel1Pose(0, 2, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel3Pose(0.3, 2, 3.0, 0, 0, 0.0); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + EXPECT_EQ(initialModel3Pose, + gz::math::eigen3::convert(frameDataModel3Body.pose)); + + // attach the fixed joint - model1 body is the parent + auto poseParent = frameDataModel1Body.pose; + auto poseChild = frameDataModel2Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint12 = model2Body->AttachFixedJoint(model1Body); + fixedJoint12->SetTransformFromParent(poseParentChild); + + poseChild = frameDataModel3Body.pose; + poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint13 = model3Body->AttachFixedJoint(model1Body); + fixedJoint13->SetTransformFromParent(poseParentChild); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect all models to remain at initial pose + EXPECT_NEAR(initialModel1Pose.Pos().Z(), + frameDataModel1Body.pose.translation().z(), 1e-3); + EXPECT_NEAR(initialModel2Pose.Pos().Z(), + frameDataModel2Body.pose.translation().z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // So it tries to resolve overlapping bodies held together by + // a fixed joint. Increase tolerance for position. + double tol = 1e-3; +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) == "bullet-featherstone") + tol = 1e-2; +#endif + EXPECT_NEAR(initialModel3Pose.Pos().Z(), + frameDataModel3Body.pose.translation().z(), tol); + + // Expect all models to have zero velocities + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // So overlapping bodies generate non-zero velocities. +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) != "bullet-featherstone") +#endif + { + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); + } + } + + // now detach joint and expect model2 and model3 to start moving + fixedJoint12->Detach(); + fixedJoint13->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the model1 to be fixed to the world and model2 and model3 + // to start moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_GT(0.0, body2LinearVelocity.Z()); + // bullet-featherstone and dartsim has different behavior + // when detaching a joint between overlapping bodies + // dartsim: body falls after joint is detached + // bullet-featherstone: pushes bodies apart + // So here we just check for non-zero velocity +#ifdef __APPLE__ + // Disable check for dartsim plugin on homebrew. + // model3 has zero velocity in dartsim on macOS. It could be a + // change in behavior between dartsim versions. model3 overlaps + // with model1 so could be stuck together + if (this->PhysicsEngineName(name) != "dartsim") +#endif + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } + + // Test attaching fixed joint with reverse the parent and child + // relationship - model2 body is now the parent and model1 is child and + // fixed to world + poseParent = frameDataModel2Body.pose; + poseChild = frameDataModel1Body.pose; + poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint12b = model1Body->AttachFixedJoint(model2Body); + fixedJoint12b->SetTransformFromParent(poseParentChild); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expect both models to have zero velocities + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + } + + // detach joint and expect model2 to start falling again + fixedJoint12b->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // Expect the model1 to still be fixed to the world and model2 + // to start falling + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + // Negative z velocity + EXPECT_GT(0.0, body2LinearVelocity.Z()); + } + } +} + +///////////////////////////////////////////////// +// Create a chain of models by attaching them with fixed joints: +// M1 (static) -> M2 (dynamic) -> M3 (dynamic) +// Verify that M2 and M3 become static once attached to M1 +TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachChain) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsFixedSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // M1 is fixed to the world + // M2 is non-static and at some distance away from M1 + // M3 is non-static and overlaps with M1 + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string modelName3{"M3"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model3 = world->GetModel(modelName3); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + auto model3Body = model3->GetLink(bodyName); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + auto frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + const gz::math::Pose3d initialModel1Pose(0, 2, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0.0); + const gz::math::Pose3d initialModel3Pose(0.3, 2, 3.0, 0, 0, 0.0); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + EXPECT_EQ(initialModel3Pose, + gz::math::eigen3::convert(frameDataModel3Body.pose)); + + // attach the fixed joint between M2 (dynamic) and M3 (dynamic) + auto poseParent = frameDataModel2Body.pose; + auto poseChild = frameDataModel3Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint23 = model3Body->AttachFixedJoint(model2Body); + fixedJoint23->SetTransformFromParent(poseParentChild); + + // attach the fixed joint between M1 (static) and M2 (dynamic) + // this should recusively make M2 and M3 static + poseParent = frameDataModel2Body.pose; + poseChild = frameDataModel3Body.pose; + poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint12 = model2Body->AttachFixedJoint(model1Body); + fixedJoint12->SetTransformFromParent(poseParentChild); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); - // Expected Z height of model2 is 0.75 when both boxes are stacked on top - // of each other since each is 0.5 high. - if (fabs(frameDataModel2Body.pose.translation().z() - 0.75) < 1e-2 && - fabs(frameDataModel2Body.linearVelocity.z()) < 1e-3) + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect all models to remain at initial pose + EXPECT_NEAR(initialModel1Pose.Pos().Z(), + frameDataModel1Body.pose.translation().z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // Increase tolerance for position. + double tol = 1e-3; +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) == "bullet-featherstone") + tol = 0.1; +#endif + EXPECT_NEAR(initialModel2Pose.Pos().Z(), + frameDataModel2Body.pose.translation().z(), tol); + EXPECT_NEAR(initialModel3Pose.Pos().Z(), + frameDataModel3Body.pose.translation().z(), tol); + + // Expect all models to have zero velocities + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + // For bullet versions <= 3.06, static collision flags are not set. + // So bodies generate non-zero velocities. +#ifdef BT_BULLET_VERSION_LE_306 + if (this->PhysicsEngineName(name) != "bullet-featherstone") +#endif { - break; + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); } } - EXPECT_GT(stepCount, 1u); - EXPECT_LT(stepCount, maxNumSteps); + + // Now detach joint between M2 (dynamic) and M3 (dynamic) + // Expect M2 to be static as it is still attached to M1 (static) + // Expect M3 to start moving + fixedJoint23->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the model1 to be fixed to the world and model2 and model3 + // to start moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-2); + // bullet-featherstone and dartsim has different behavior + // when detaching a joint between overlapping bodies + // dartsim: body falls after joint is detached + // bullet-featherstone: pushes bodies apart + // So here we just check for non-zero velocity +#ifdef __APPLE__ + // Disable check for dartsim plugin on homebrew. + // model3 has zero velocity in dartsim on macOS. It could be a + // change in behavior between dartsim versions. model3 overlaps + // with model1 so could be stuck together + if (this->PhysicsEngineName(name) != "dartsim") +#endif + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } + + // Now detach joint between M1 (static) and M2 (dynamic) + // Expect M2 to start falling + // Expect M3 to continue moving + fixedJoint12->Detach(); + // fixedJoint13->Detach(); + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the model1 to be fixed to the world and model2 and model3 + // to start moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_GT(0.0, body2LinearVelocity.Z()); + // bullet-featherstone and dartsim has different behavior + // when detaching a joint between overlapping bodies + // dartsim: body falls after joint is detached + // bullet-featherstone: pushes bodies apart + // So here we just check for non-zero velocity +#ifdef __APPLE__ + // Disable check for dartsim plugin on homebrew. + // model3 has zero velocity in dartsim on macOS. It could be a + // change in behavior between dartsim versions. model3 overlaps + // with model1 so could be stuck together + if (this->PhysicsEngineName(name) != "dartsim") +#endif + EXPECT_NE(gz::math::Vector3d::Zero, body3LinearVelocity); + } } } @@ -1269,8 +1648,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) EXPECT_EQ(initialModel3Pose, gz::math::eigen3::convert(frameDataModel3Body.pose)); - // Step through initial transients const std::size_t numSteps = 100; + /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); @@ -1283,18 +1662,15 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect all the bodies to be at rest. // (since they're held in place by the joints) - { - world->Step(output, state, input); - EXPECT_NEAR(initialModel1Pose.Z(), - frameDataModel1Body.pose.translation().z(), 1e-3); - EXPECT_NEAR(initialModel2Pose.Z(), - frameDataModel2Body.pose.translation().z(), 1e-3); - EXPECT_NEAR(initialModel3Pose.Z(), - frameDataModel3Body.pose.translation().z(), 1e-3); - EXPECT_NEAR(0.0, frameDataModel1Body.linearVelocity.z(), 3e-3); - EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); - EXPECT_NEAR(0.0, frameDataModel3Body.linearVelocity.z(), 3e-3); - } + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); } // Detach the joints. M1 and M3 should fall as there is now nothing stopping @@ -1302,13 +1678,6 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) fixedJoint_m2m1->Detach(); fixedJoint_m2m3->Detach(); - frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); - frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); - const double preStepBody1LinearVelocityZ = - frameDataModel1Body.linearVelocity.z(); - const double preStepBody3LinearVelocityZ = - frameDataModel3Body.linearVelocity.z(); - /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { @@ -1322,11 +1691,16 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect the middle box to be still as it is already at rest. // Expect the two side boxes to fall away. - EXPECT_NEAR(preStepBody1LinearVelocityZ + dt * (numSteps) * -10, - frameDataModel1Body.linearVelocity.z(), 1e-3); - EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); - EXPECT_NEAR(preStepBody3LinearVelocityZ + dt * (numSteps) * -10, - frameDataModel3Body.linearVelocity.z(), 1e-3); + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + + EXPECT_NEAR(dt * (numSteps) * -10, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(dt * (numSteps) * -10, body3LinearVelocity.Z(), 1e-3); } } } @@ -1638,6 +2012,124 @@ TEST_F(WorldModelTest, JointSetCommand) } } +using FixedJointFreeGroupFeatureList = gz::physics::FeatureList< + gz::physics::FindFreeGroupFeature, + gz::physics::SetFreeGroupWorldPose, + gz::physics::AttachFixedJointFeature, + gz::physics::DetachJointFeature, + gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, + gz::physics::LinkFrameSemantics, + gz::physics::SetBasicJointState, + gz::physics::SetJointTransformFromParentFeature, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld +>; + +using FixedJointFreeGroupFeatureTestTypes = + JointFeaturesTest; + +TEST_F(FixedJointFreeGroupFeatureTestTypes, FixedJointFreeGroupMove) +{ + // Attach joint between links from 2 models with fixed joint. Move + // parent model using free group and verify child body moves along with the + // parent link. + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = + gz::physics::RequestEngine3d::From( + plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + + const gz::math::Pose3d model1Pose(0, 0, 0.25, 0, 0, 0.0); + const gz::math::Pose3d model2Pose(0, 1, 0.25, 0, 0, 0.0); + auto freeGroupM1 = model1->FindFreeGroup(); + auto freeGroupM2 = model2->FindFreeGroup(); + freeGroupM1->SetWorldPose(gz::math::eigen3::convert(model1Pose)); + freeGroupM2->SetWorldPose(gz::math::eigen3::convert(model2Pose)); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + EXPECT_EQ(model1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(model2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expect the model1 and model2 to stay at rest + // (since they are on the ground) + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-2); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-2); + } + + // Attach fixed joint + const auto poseParent = frameDataModel1Body.pose; + const auto poseChild = frameDataModel2Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint = model2Body->AttachFixedJoint(model1Body); + fixedJoint->SetTransformFromParent(poseParentChild); + + gz::math::Pose3d poseToMoveModel(1, 0, 0, 0, 0, 0.0); + gz::math::Pose3d newModel1Pose = poseToMoveModel * model1Pose; + gz::math::Pose3d newModel2Pose = poseToMoveModel * model2Pose; + + // Move parent model using free group + freeGroupM1->SetWorldPose(gz::math::eigen3::convert(newModel1Pose)); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // Parent should be at the new pose and the child should follow + EXPECT_EQ(newModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(newModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/worlds/joint_across_models_fixed.sdf b/test/common_test/worlds/joint_across_models_fixed.sdf new file mode 100644 index 000000000..a6e355550 --- /dev/null +++ b/test/common_test/worlds/joint_across_models_fixed.sdf @@ -0,0 +1,145 @@ + + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 3.0 0 0.0 0.0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + world + body + + + + 0 0 3.0 0 0.0 0.0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0.3 2 3.0 0 0.0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + From 1d9ede3e4b879c724d5df70aea9fb6e6031953e2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 5 Jun 2024 22:53:37 +0800 Subject: [PATCH 7/9] bullet-featherstone: Fix bounding box for collisions with pose offset (#647) Currently if a collision has a pose offset, the bullet-featherstone plugin returns an incorrect axis aligned bounding box for the link containing the collision. This is found to be caused by incorrect frame data pose computed for collisions - It currently ignores the collision pose offset. This PR makes a few changes to KinematicsFeatures::FrameDataRelativeToWorld in bullet-featherstone plugin: - fixed getting collision pose relative to world - fixed getting model pose relative to world - minor refactoring The link_features's bounding box test was previously skipped for bullet-featherstone plugin. This PR enables this test for bullet-featherstone by requiring only a minimal set of features needed to run the test. --------- Signed-off-by: Ian Chen --- bullet-featherstone/src/KinematicsFeatures.cc | 103 +++++++++--------- bullet-featherstone/src/KinematicsFeatures.hh | 3 +- bullet-featherstone/src/ShapeFeatures.cc | 2 - test/common_test/Worlds.hh | 1 + test/common_test/kinematic_features.cc | 86 ++++++++++++++- test/common_test/link_features.cc | 22 ++-- test/common_test/worlds/pose_offset.sdf | 46 ++++++++ 7 files changed, 197 insertions(+), 66 deletions(-) create mode 100644 test/common_test/worlds/pose_offset.sdf diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc index 85341ce6a..c65cc5b48 100644 --- a/bullet-featherstone/src/KinematicsFeatures.cc +++ b/bullet-featherstone/src/KinematicsFeatures.cc @@ -22,30 +22,40 @@ namespace gz { namespace physics { namespace bullet_featherstone { + +FrameData3d getNonBaseLinkFrameData(const ModelInfo *_modelInfo, + const LinkInfo *_linkInfo) +{ + const auto index = _linkInfo->indexInModel.value(); + FrameData3d data; + data.pose = GetWorldTransformOfLink(*_modelInfo, *_linkInfo); + const auto &link = _modelInfo->body->getLink(index); + data.linearVelocity = convert(link.m_absFrameTotVelocity.getLinear()); + data.angularVelocity = convert(link.m_absFrameTotVelocity.getAngular()); + return data; +} + ///////////////////////////////////////////////// FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( const FrameID &_id) const { - const auto linkIt = this->links.find(_id.ID()); + bool isModel = false; + bool isCollision = false; const ModelInfo *model = nullptr; + Eigen::Isometry3d collisionPoseOffset = Eigen::Isometry3d(); + + const auto linkIt = this->links.find(_id.ID()); if (linkIt != this->links.end()) { const auto &linkInfo = linkIt->second; - const auto indexOpt = linkInfo->indexInModel; model = this->ReferenceInterface(linkInfo->model); - if (indexOpt.has_value()) + if (linkInfo->indexInModel.has_value()) { - const auto index = *indexOpt; - FrameData data; - data.pose = GetWorldTransformOfLink(*model, *linkInfo); - const auto &link = model->body->getLink(index); - data.linearVelocity = convert(link.m_absFrameTotVelocity.getLinear()); - data.angularVelocity = convert(link.m_absFrameTotVelocity.getAngular()); - return data; + return getNonBaseLinkFrameData(model, linkInfo.get()); } - // If indexOpt is nullopt then the link is the base link which will be + // If indexInModel is nullopt then the link is the base link which will be // calculated below. } else @@ -59,60 +69,55 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( if (linkIt2 != this->links.end()) { const auto &linkInfo2 = linkIt2->second; - const auto indexOpt2 = linkInfo2->indexInModel; model = this->ReferenceInterface(linkInfo2->model); - - if (indexOpt2.has_value()) + if (linkInfo2->indexInModel.has_value()) { - const auto index2 = *indexOpt2; - FrameData data; - data.pose = GetWorldTransformOfLink(*model, *linkInfo2); - const auto &link = model->body->getLink(index2); - data.linearVelocity = convert( - link.m_absFrameTotVelocity.getLinear()); - data.angularVelocity = convert( - link.m_absFrameTotVelocity.getAngular()); - return data; + return getNonBaseLinkFrameData(model, linkInfo2.get()); } } } - - auto collisionIt = this->collisions.find(_id.ID()); - if (collisionIt != this->collisions.end()) + else { - const auto &collisionInfo = collisionIt->second; - - const auto linkIt2 = this->links.find(collisionInfo->link); - if (linkIt2 != this->links.end()) + auto collisionIt = this->collisions.find(_id.ID()); + if (collisionIt != this->collisions.end()) { - const auto &linkInfo2 = linkIt2->second; - const auto indexOpt2 = linkInfo2->indexInModel; - model = this->ReferenceInterface(linkInfo2->model); + isCollision = true; + const auto &collisionInfo = collisionIt->second; - if (indexOpt2.has_value()) + const auto linkIt2 = this->links.find(collisionInfo->link); + if (linkIt2 != this->links.end()) { - const auto index2 = *indexOpt2; - FrameData data; - data.pose = GetWorldTransformOfLink(*model, *linkInfo2); - const auto &link = model->body->getLink(index2); - data.linearVelocity = convert( - link.m_absFrameTotVelocity.getLinear()); - data.angularVelocity = convert( - link.m_absFrameTotVelocity.getAngular()); - return data; + const auto &linkInfo2 = linkIt2->second; + model = this->ReferenceInterface(linkInfo2->model); + collisionPoseOffset = collisionInfo->linkToCollision; + if (linkInfo2->indexInModel.has_value()) + { + auto data = getNonBaseLinkFrameData(model, linkInfo2.get()); + data.pose = data.pose * collisionPoseOffset; + return data; + } + } + } + else + { + auto modelIt = this->models.find(_id.ID()); + if (modelIt != this->models.end()) + { + model = modelIt->second.get(); + isModel = true; } } } - - if (!model || model->body == nullptr) - model = this->FrameInterface(_id); } FrameData data; - if(model && model->body) + if (model && model->body) { - data.pose = convert(model->body->getBaseWorldTransform()) - * model->baseInertiaToLinkFrame; + data.pose = convert(model->body->getBaseWorldTransform()); + if (!isModel) + data.pose = data.pose * model->baseInertiaToLinkFrame; + if (isCollision) + data.pose = data.pose * collisionPoseOffset; data.linearVelocity = convert(model->body->getBaseVel()); data.angularVelocity = convert(model->body->getBaseOmega()); } diff --git a/bullet-featherstone/src/KinematicsFeatures.hh b/bullet-featherstone/src/KinematicsFeatures.hh index 259db0dd2..54ff7554c 100644 --- a/bullet-featherstone/src/KinematicsFeatures.hh +++ b/bullet-featherstone/src/KinematicsFeatures.hh @@ -30,7 +30,8 @@ namespace bullet_featherstone { struct KinematicsFeatureList : gz::physics::FeatureList< LinkFrameSemantics, ModelFrameSemantics, - FreeGroupFrameSemantics + FreeGroupFrameSemantics, + ShapeFrameSemantics > { }; class KinematicsFeatures : diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc index 1f05f51fc..34f78007c 100644 --- a/bullet-featherstone/src/ShapeFeatures.cc +++ b/bullet-featherstone/src/ShapeFeatures.cc @@ -37,8 +37,6 @@ AlignedBox3d ShapeFeatures::GetShapeAxisAlignedBoundingBox( t.setIdentity(); btVector3 minBox(0, 0, 0); btVector3 maxBox(0, 0, 0); - btVector3 minBox2(0, 0, 0); - btVector3 maxBox2(0, 0, 0); collider->collider->getAabb(t, minBox, maxBox); return math::eigen3::convert(math::AxisAlignedBox( math::Vector3d(minBox[0], minBox[1], minBox[2]), diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index 2328cb033..326ac201d 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -47,6 +47,7 @@ const auto kMimicUniversalWorld = CommonTestWorld("mimic_universal_world.sdf"); const auto kMultipleCollisionsSdf = CommonTestWorld("multiple_collisions.sdf"); const auto kPendulumJointWrenchSdf = CommonTestWorld("pendulum_joint_wrench.sdf"); +const auto kPoseOffsetSdf = CommonTestWorld("pose_offset.sdf"); const auto kShapesWorld = CommonTestWorld("shapes.world"); const auto kShapesBitmaskWorld = CommonTestWorld("shapes_bitmask.sdf"); const auto kSlipComplianceSdf = CommonTestWorld("slip_compliance.sdf"); diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index effa773dc..3b5459bee 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include "test/TestLibLoader.hh" @@ -29,6 +30,7 @@ #include #include #include +#include #include #include @@ -72,11 +74,13 @@ struct KinematicFeaturesList : gz::physics::FeatureList< gz::physics::GetEngineInfo, gz::physics::ForwardStep, gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetShapeFromLink, gz::physics::GetModelFromWorld, gz::physics::GetLinkFromModel, gz::physics::GetJointFromModel, + gz::physics::JointFrameSemantics, gz::physics::LinkFrameSemantics, - gz::physics::JointFrameSemantics + gz::physics::ShapeFrameSemantics > { }; using KinematicFeaturesTestTypes = @@ -146,12 +150,18 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) EXPECT_EQ( F_WCexpected.pose.rotation(), childLinkFrameData.pose.rotation()); - // TODO(ahcorde): Rewiew this in bullet-featherstone - if(this->PhysicsEngineName(name) == "bullet_featherstone") + // TODO(ahcorde): Review this in bullet-featherstone + if (this->PhysicsEngineName(name) != "bullet-featherstone") { - EXPECT_EQ( - F_WCexpected.pose.translation(), - childLinkFrameData.pose.translation()); + EXPECT_NEAR( + F_WCexpected.pose.translation().x(), + childLinkFrameData.pose.translation().x(), 1e-6); + EXPECT_NEAR( + F_WCexpected.pose.translation().y(), + childLinkFrameData.pose.translation().y(), 1e-6); + EXPECT_NEAR( + F_WCexpected.pose.translation().z(), + childLinkFrameData.pose.translation().z(), 1e-6); } EXPECT_TRUE( gz::physics::test::Equal( @@ -166,6 +176,70 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) } } +TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) +{ + 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::kPoseOffsetSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + auto model = world->GetModel("model"); + ASSERT_NE(nullptr, model); + auto baseLink = model->GetLink("base"); + ASSERT_NE(nullptr, baseLink); + auto nonBaseLink = model->GetLink("link"); + ASSERT_NE(nullptr, nonBaseLink); + auto baseCol = baseLink->GetShape("base_collision"); + ASSERT_NE(nullptr, baseCol); + auto linkCol = nonBaseLink->GetShape("link_collision"); + ASSERT_NE(nullptr, linkCol); + + gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0); + auto baseLinkFrameData = baseLink->FrameDataRelativeToWorld(); + auto baseLinkPose = gz::math::eigen3::convert(baseLinkFrameData.pose); + gz::math::Pose3d actualLinkLocalPose(0, 1, 0, 0, 0, 0); + gz::math::Pose3d expectedLinkWorldPose = + actualModelPose * actualLinkLocalPose; + EXPECT_EQ(expectedLinkWorldPose, baseLinkPose); + + auto baseColFrameData = baseCol->FrameDataRelativeToWorld(); + auto baseColPose = gz::math::eigen3::convert(baseColFrameData.pose); + gz::math::Pose3d actualColLocalPose(0, 0, 0.01, 0, 0, 0); + gz::math::Pose3d expectedColWorldPose = + actualModelPose * actualLinkLocalPose * actualColLocalPose; + EXPECT_EQ(expectedColWorldPose.Pos(), baseColPose.Pos()); + EXPECT_EQ(expectedColWorldPose.Rot().Euler(), + baseColPose.Rot().Euler()); + + auto nonBaseLinkFrameData = nonBaseLink->FrameDataRelativeToWorld(); + auto nonBaseLinkPose = gz::math::eigen3::convert(nonBaseLinkFrameData.pose); + actualLinkLocalPose = gz::math::Pose3d (0, 0, 2.1, -1.5708, 0, 0); + expectedLinkWorldPose = actualModelPose * actualLinkLocalPose; + EXPECT_EQ(expectedLinkWorldPose, nonBaseLinkPose); + + auto linkColFrameData = linkCol->FrameDataRelativeToWorld(); + auto linkColPose = gz::math::eigen3::convert(linkColFrameData.pose); + actualColLocalPose = gz::math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0); + expectedColWorldPose = + actualModelPose * actualLinkLocalPose * actualColLocalPose; + EXPECT_EQ(expectedColWorldPose.Pos(), linkColPose.Pos()); + EXPECT_EQ(expectedColWorldPose.Rot().Euler(), + linkColPose.Rot().Euler()); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/link_features.cc b/test/common_test/link_features.cc index adcb54817..5f5ae3112 100644 --- a/test/common_test/link_features.cc +++ b/test/common_test/link_features.cc @@ -293,14 +293,26 @@ TYPED_TEST(LinkFeaturesTest, JointSetCommand) } } -TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox) +using LinkBoundingBoxFeaturesList = gz::physics::FeatureList< + gz::physics::ForwardStep, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetEntities, + gz::physics::GetLinkBoundingBox, + gz::physics::GetModelBoundingBox +>; + +using LinkBoundingBoxFeaturesTestTypes = + LinkFeaturesTest; + +TEST_F(LinkBoundingBoxFeaturesTestTypes, AxisAlignedBoundingBox) { 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); + auto engine = + gz::physics::RequestEngine3d::From(plugin); ASSERT_NE(nullptr, engine); sdf::Root root; @@ -313,9 +325,6 @@ TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox) auto world = engine->ConstructWorld(*root.WorldByIndex(0)); EXPECT_NE(nullptr, world); - EXPECT_NE(nullptr, world); - world->SetGravity(Eigen::Vector3d{0, 0, -9.8}); - auto model = world->GetModel("double_pendulum_with_base"); auto baseLink = model->GetLink("base"); auto bbox = baseLink->GetAxisAlignedBoundingBox(); @@ -364,9 +373,6 @@ TYPED_TEST(LinkFeaturesTest, ModelAxisAlignedBoundingBox) auto world = engine->ConstructWorld(*root.WorldByIndex(0)); EXPECT_NE(nullptr, world); - EXPECT_NE(nullptr, world); - world->SetGravity(Eigen::Vector3d{0, 0, -9.8}); - auto model = world->GetModel("sphere"); auto bbox = model->GetAxisAlignedBoundingBox(); AssertVectorApprox vectorPredicate(1e-4); diff --git a/test/common_test/worlds/pose_offset.sdf b/test/common_test/worlds/pose_offset.sdf new file mode 100644 index 000000000..97b2145cd --- /dev/null +++ b/test/common_test/worlds/pose_offset.sdf @@ -0,0 +1,46 @@ + + + + + 1 0 0 0 0 0 + + 0 1 0 0 0 0 + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + + base + link + + 1.0 0 0 + + + + + From b5d1508bb7011240d64755506b599c5cd3f18ffa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=B4mulo=20Cerqueira?= Date: Wed, 12 Jun 2024 14:10:13 -0300 Subject: [PATCH 8/9] Ray intersection simulation feature (#641) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit dartsim does support ray-based collisions via Bullet backend, which is also supported by gz-physics. This PR creates a simulation feature to compute and retrieve ray castings. Also, it updates dartsim version >= 6.10, which fixes the issues when no ray hit. This addition is useful for range-based applications (e.g. laser, altimeter etc.). --------- Signed-off-by: Rômulo Cerqueira Co-authored-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- dartsim/src/SimulationFeatures.cc | 40 ++++++++ dartsim/src/SimulationFeatures.hh | 12 ++- include/gz/physics/GetRayIntersection.hh | 86 ++++++++++++++++ .../gz/physics/detail/GetRayIntersection.hh | 49 ++++++++++ test/common_test/simulation_features.cc | 98 +++++++++++++++++++ 6 files changed, 285 insertions(+), 2 deletions(-) create mode 100644 include/gz/physics/GetRayIntersection.hh create mode 100644 include/gz/physics/detail/GetRayIntersection.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index aa41e2dcb..fd48ffb51 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,7 +78,7 @@ gz_find_package(DART utils utils-urdf CONFIG - VERSION 6.9 + VERSION 6.10 REQUIRED_BY dartsim PKGCONFIG dart PKGCONFIG_VER_COMPARISON >=) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 47caa9b0b..9475ef984 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -15,6 +15,7 @@ * */ +#include #include #include #include @@ -165,6 +166,45 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const this->prevLinkPoses = std::move(newPoses); } +SimulationFeatures::RayIntersection +SimulationFeatures::GetRayIntersectionFromLastStep( + const Identity &_worldID, + const LinearVector3d &_from, + const LinearVector3d &_to) const +{ + auto *const world = this->ReferenceInterface(_worldID); + auto collisionDetector = world->getConstraintSolver()->getCollisionDetector(); + auto collisionGroup = world->getConstraintSolver()->getCollisionGroup().get(); + + // Perform raycast + dart::collision::RaycastOption option; + dart::collision::RaycastResult result; + collisionDetector->raycast(collisionGroup, _from, _to, option, &result); + + // Currently, raycast supports only the Bullet collision detector. + // For other collision detectors, the result will always be NaN. + SimulationFeatures::RayIntersection intersection; + if (result.hasHit()) + { + // Store intersection data if there is a ray hit + const auto &firstHit = result.mRayHits[0]; + intersection.point = firstHit.mPoint; + intersection.normal = firstHit.mNormal; + intersection.fraction = firstHit.mFraction; + } + else + { + // Set invalid measurements to NaN according to REP-117 + intersection.point = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + intersection.normal = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + intersection.fraction = std::numeric_limits::quiet_NaN(); + } + + return intersection; +} + std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index c13873837..a3365feb8 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -56,7 +57,8 @@ struct SimulationFeatureList : FeatureList< #ifdef DART_HAS_CONTACT_SURFACE SetContactPropertiesCallbackFeature, #endif - GetContactsFromLastStepFeature + GetContactsFromLastStepFeature, + GetRayIntersectionFromLastStepFeature > { }; #ifdef DART_HAS_CONTACT_SURFACE @@ -97,6 +99,9 @@ class SimulationFeatures : public: using GetContactsFromLastStepFeature::Implementation ::ContactInternal; + public: using GetRayIntersectionFromLastStepFeature::Implementation< + FeaturePolicy3d>::RayIntersection; + public: SimulationFeatures() = default; public: ~SimulationFeatures() override = default; @@ -113,6 +118,11 @@ class SimulationFeatures : public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; + public: RayIntersection GetRayIntersectionFromLastStep( + const Identity &_worldID, + const LinearVector3d &_from, + const LinearVector3d &_end) const override; + /// \brief link poses from the most recent pose change/update. /// The key is the link's ID, and the value is the link's pose private: mutable std::unordered_map prevLinkPoses; diff --git a/include/gz/physics/GetRayIntersection.hh b/include/gz/physics/GetRayIntersection.hh new file mode 100644 index 000000000..1c9e31223 --- /dev/null +++ b/include/gz/physics/GetRayIntersection.hh @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef GZ_PHYSICS_GETRAYINTERSECTION_HH_ +#define GZ_PHYSICS_GETRAYINTERSECTION_HH_ + +#include +#include +#include +#include + +namespace gz +{ +namespace physics +{ +/// \brief GetRayIntersectionFromLastStepFeature is a feature for retrieving +/// the ray intersection generated in the previous simulation step. +class GZ_PHYSICS_VISIBLE GetRayIntersectionFromLastStepFeature + : public virtual FeatureWithRequirements +{ + public: template + struct RayIntersectionT + { + public: using Scalar = typename PolicyT::Scalar; + public: using VectorType = + typename FromPolicy::template Use; + + /// \brief The hit point in the world coordinates + VectorType point; + + /// \brief The fraction of the ray length at the intersection/hit point. + Scalar fraction; + + /// \brief The normal at the hit point in the world coordinates + VectorType normal; + }; + + public: template + class World : public virtual Feature::World + { + public: using VectorType = + typename FromPolicy::template Use; + public: using RayIntersection = RayIntersectionT; + public: using RayIntersectionData = + SpecifyData>; + + /// \brief Get ray intersection generated in the previous simulation step + /// \param[in] _from The start point of the ray in world coordinates + /// \param[in] _to The end point of the ray in world coordinates + public: RayIntersectionData GetRayIntersectionFromLastStep( + const VectorType &_from, const VectorType &_to) const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using RayIntersection = RayIntersectionT; + public: using VectorType = + typename FromPolicy::template Use; + + public: virtual RayIntersection GetRayIntersectionFromLastStep( + const Identity &_worldID, + const VectorType &_from, + const VectorType &_to) const = 0; + }; +}; +} +} + +#include "gz/physics/detail/GetRayIntersection.hh" + +#endif /* end of include guard: GZ_PHYSICS_GETRAYINTERSECTION_HH_ */ diff --git a/include/gz/physics/detail/GetRayIntersection.hh b/include/gz/physics/detail/GetRayIntersection.hh new file mode 100644 index 000000000..8448ea608 --- /dev/null +++ b/include/gz/physics/detail/GetRayIntersection.hh @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef GZ_PHYSICS_DETAIL_GETRAYINTERSECTION_HH_ +#define GZ_PHYSICS_DETAIL_GETRAYINTERSECTION_HH_ + +#include +#include + +namespace gz +{ +namespace physics +{ +///////////////////////////////////////////////// +template +auto GetRayIntersectionFromLastStepFeature::World< + PolicyT, FeaturesT>::GetRayIntersectionFromLastStep( + const VectorType &_from, + const VectorType &_to) const -> RayIntersectionData +{ + auto result = + this->template Interface() + ->GetRayIntersectionFromLastStep(this->identity, _from, _to); + + RayIntersection intersection{result.point, result.fraction, result.normal}; + + RayIntersectionData output; + output.template Get() = std::move(intersection); + return output; +} + +} // namespace physics +} // namespace gz + +#endif diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index d92e474f9..83afc7bdd 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -37,6 +37,7 @@ #include "gz/physics/BoxShape.hh" #include +#include #include "gz/physics/ContactProperties.hh" #include "gz/physics/CylinderShape.hh" #include "gz/physics/CapsuleShape.hh" @@ -1233,6 +1234,103 @@ TYPED_TEST(SimulationFeaturesTestBasic, MultipleCollisions) } } +///////////////////////////////////////////////// +// The features that an engine must have to be loaded by this loader. +struct FeaturesRayIntersections : gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetRayIntersectionFromLastStepFeature, + gz::physics::CollisionDetector, + gz::physics::ForwardStep +> {}; + +template +class SimulationFeaturesRayIntersectionTest : + public SimulationFeaturesTest{}; +using SimulationFeaturesRayIntersectionTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesRayIntersectionTest, + SimulationFeaturesRayIntersectionTestTypes); + +TYPED_TEST(SimulationFeaturesRayIntersectionTest, SupportedRayIntersections) +{ + std::vector supportedCollisionDetectors = {"bullet"}; + + for (const std::string &name : this->pluginNames) + { + CHECK_UNSUPPORTED_ENGINE(name, "bullet", "bullet-featherstone", "tpe") + + for (const std::string &collisionDetector : supportedCollisionDetectors) { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector(collisionDetector); + auto checkedOutput = StepWorld(world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + // ray hits the sphere + auto result = + world->GetRayIntersectionFromLastStep( + Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2)); + + auto rayIntersection = + result.template + Get::RayIntersection>(); + + double epsilon = 1e-3; + EXPECT_TRUE( + rayIntersection.point.isApprox(Eigen::Vector3d(-1, 0, 2), epsilon)); + EXPECT_TRUE( + rayIntersection.normal.isApprox(Eigen::Vector3d(-1, 0, 0), epsilon)); + EXPECT_DOUBLE_EQ(rayIntersection.fraction, 0.25); + + // ray does not hit the sphere + result = world->GetRayIntersectionFromLastStep( + Eigen::Vector3d(2, 0, 10), Eigen::Vector3d(-2, 0, 10)); + rayIntersection = + result.template + Get::RayIntersection>(); + + ASSERT_TRUE(rayIntersection.point.array().isNaN().any()); + ASSERT_TRUE(rayIntersection.normal.array().isNaN().any()); + ASSERT_TRUE(std::isnan(rayIntersection.fraction)); + } + } +} + +TYPED_TEST(SimulationFeaturesRayIntersectionTest, UnsupportedRayIntersections) +{ + std::vector unsupportedCollisionDetectors = {"ode", "dart", "fcl", "banana"}; + + for (const std::string &name : this->pluginNames) + { + CHECK_UNSUPPORTED_ENGINE(name, "bullet", "bullet-featherstone", "tpe") + + for (const std::string &collisionDetector : unsupportedCollisionDetectors) { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector(collisionDetector); + auto checkedOutput = StepWorld(world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + // ray would hit the sphere, but the collision detector does + // not support ray intersection + auto result = world->GetRayIntersectionFromLastStep( + Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2)); + + auto rayIntersection = + result.template + Get::RayIntersection>(); + + ASSERT_TRUE(rayIntersection.point.array().isNaN().any()); + ASSERT_TRUE(rayIntersection.normal.array().isNaN().any()); + ASSERT_TRUE(std::isnan(rayIntersection.fraction)); + } + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); From 6bcf1af48b016a1e8300e0eed6dcd008fa044cea Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 13 Jun 2024 17:30:45 -0700 Subject: [PATCH 9/9] bullet-featherstone: Fix attaching fixed joint between models with inertial pose offset (#653) When creating a fixed constraint between links, the SetJointTransformFromParent currently does not take into account the inertial pose. If an inertial pose offset exists, the fixed constraint causes the 2 models to be displaced from their original position. This PR updates the calculation in SetJointTransformFromParent to include inertial pose offset. The detachable_joint.world test world is updated to use a model with inertial pose offset - this world is used by the CorrectAttachmentPoints test in detachable_joint common test. The test fails without the changes in this PR. --------- Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 18 +++++++++++++----- test/common_test/worlds/detachable_joint.world | 1 + 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 9618bb20c..e2c460a32 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -509,11 +509,19 @@ void JointFeatures::SetJointTransformFromParent( if (jointInfo->fixedConstraint) { - auto tf = convertTf(_pose); - jointInfo->fixedConstraint->setPivotInA( - tf.getOrigin()); - jointInfo->fixedConstraint->setFrameInA( - tf.getBasis()); + Eigen::Isometry3d parentInertiaToLinkFrame = Eigen::Isometry3d::Identity(); + if (jointInfo->parentLinkID.has_value()) + { + auto parentLinkInfo = this->links.at(jointInfo->parentLinkID.value()); + parentInertiaToLinkFrame = parentLinkInfo->inertiaToLinkFrame; + } + auto *linkInfo = this->ReferenceInterface(jointInfo->childLinkID); + auto tf = convertTf(parentInertiaToLinkFrame * _pose * + linkInfo->inertiaToLinkFrame.inverse()); + jointInfo->fixedConstraint->setPivotInA( + tf.getOrigin()); + jointInfo->fixedConstraint->setFrameInA( + tf.getBasis()); } } diff --git a/test/common_test/worlds/detachable_joint.world b/test/common_test/worlds/detachable_joint.world index b7e4c5310..e6922e931 100644 --- a/test/common_test/worlds/detachable_joint.world +++ b/test/common_test/worlds/detachable_joint.world @@ -101,6 +101,7 @@ 1.0 + 0.0 0 0.3 0 0 0 0.0068 0