diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index b81f17330ac..8cd8991eed6 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -3742,12 +3743,21 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) return; } + // Using ExtraContactData to expose contact Norm, Force & Depth + using Policy = physics::FeaturePolicy3d; + using GCFeature = physics::GetContactsFromLastStepFeature; + using ExtraContactData = GCFeature::ExtraContactDataT; + + // A contact is described by a contactPoint and the corresponding + // extraContactData which we bundle in a pair data structure + using ContactData = std::pair; // Each contact object we get from gz-physics contains the EntityPtrs of the // two colliding entities and other data about the contact such as the - // position. This map groups contacts so that it is easy to query all the + // position and extra contact date (wrench, normal and penetration depth). + // This map groups contacts so that it is easy to query all the // contacts of one entity. - using EntityContactMap = std::unordered_map>; + using EntityContactMap = std::unordered_map>; // This data structure is essentially a mapping between a pair of entities and // a list of pointers to their contact object. We use a map inside a map to @@ -3761,16 +3771,19 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contactComposite : allContacts) { - const auto &contact = contactComposite.Get(); - auto coll1Entity = - this->entityCollisionMap.GetByPhysicsId(contact.collision1->EntityID()); - auto coll2Entity = - this->entityCollisionMap.GetByPhysicsId(contact.collision2->EntityID()); + const auto &contactPoint = + contactComposite.Get(); + const auto &extraContactData = contactComposite.Query(); + auto coll1Entity = this->entityCollisionMap.GetByPhysicsId( + contactPoint.collision1->EntityID()); + auto coll2Entity = this->entityCollisionMap.GetByPhysicsId( + contactPoint.collision2->EntityID()); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) { - entityContactMap[coll1Entity][coll2Entity].push_back(&contact); - entityContactMap[coll2Entity][coll1Entity].push_back(&contact); + ContactData data = std::make_pair(&contactPoint, extraContactData); + entityContactMap[coll1Entity][coll2Entity].push_back(data); + entityContactMap[coll2Entity][coll1Entity].push_back(data); } } @@ -3811,9 +3824,36 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contact : contactData) { auto *position = contactMsg->add_position(); - position->set_x(contact->point.x()); - position->set_y(contact->point.y()); - position->set_z(contact->point.z()); + position->set_x(contact.first->point.x()); + position->set_y(contact.first->point.y()); + position->set_z(contact.first->point.z()); + + // Check if the extra contact data exists, + // since not all physics engines support it. + // Then, fill the msg with extra data. + if(contact.second != nullptr) + { + auto *normal = contactMsg->add_normal(); + normal->set_x(contact.second->normal.x()); + normal->set_y(contact.second->normal.y()); + normal->set_z(contact.second->normal.z()); + + auto *wrench = contactMsg->add_wrench(); + auto *body1Wrench = wrench->mutable_body_1_wrench(); + auto *body1Force = body1Wrench->mutable_force(); + body1Force->set_x(contact.second->force.x()); + body1Force->set_y(contact.second->force.y()); + body1Force->set_z(contact.second->force.z()); + + // The force on the second body is equal and opposite + auto *body2Wrench = wrench->mutable_body_2_wrench(); + auto *body2Force = body2Wrench->mutable_force(); + body2Force->set_x(-contact.second->force.x()); + body2Force->set_y(-contact.second->force.y()); + body2Force->set_z(-contact.second->force.z()); + + contactMsg->add_depth(contact.second->depth); + } } } diff --git a/test/integration/contact_system.cc b/test/integration/contact_system.cc index 10431e496eb..2ac7694388d 100644 --- a/test/integration/contact_system.cc +++ b/test/integration/contact_system.cc @@ -272,6 +272,99 @@ TEST_F(ContactSystemTest, } } +///////////////////////////////////////////////// +// The test checks that contacts are published with +// the correct extraContactData +TEST_F(ContactSystemTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(ExtraContactData)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/contact_extra_data.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::mutex contactMutex; + std::vector contactMsgs; + + auto contactCb = [&](const msgs::Contacts &_msg) -> void + { + std::lock_guard lock(contactMutex); + contactMsgs.push_back(_msg); + }; + + // subscribe to contacts topic + transport::Node node; + // Have to create an lvalue here for Node::Subscribe to work. + auto callbackFunc = std::function(contactCb); + node.Subscribe("/test_extra_collision_data", callbackFunc); + + // Run server for 10 iters to ensure + // contact solver has converged + size_t iters = 10; + server.Run(true, iters, false); + { + std::lock_guard lock(contactMutex); + ASSERT_GE(contactMsgs.size(), 1u); + } + + { + std::lock_guard lock(contactMutex); + const auto &lastContacts = contactMsgs.back(); + EXPECT_EQ(1, lastContacts.contact_size()); + + // The sphere weighs 1kg and gravity is set to 9.81 m/s^2 + // Hence the contact force should be m*g = 9.81 N + // along the z-axis. The force on body 2 should be equal + // and opposite. + // All torques should be zero. + // The normal should align with the world z-axis + for (const auto &contact : lastContacts.contact()) + { + ASSERT_EQ(1, contact.wrench_size()); + ASSERT_EQ(1, contact.normal_size()); + + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().force().x(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().force().y(), + 5e-2); + EXPECT_NEAR(9.81, contact.wrench().Get(0).body_1_wrench().force().z(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().x(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().y(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_1_wrench().torque().z(), + 5e-2); + + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().force().x(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().force().y(), + 5e-2); + EXPECT_NEAR(-9.81, contact.wrench().Get(0).body_2_wrench().force().z(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().x(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().y(), + 5e-2); + EXPECT_NEAR(0.0, contact.wrench().Get(0).body_2_wrench().torque().z(), + 5e-2); + + EXPECT_NEAR(0.0, contact.normal().Get(0).x(), 5e-2); + EXPECT_NEAR(0.0, contact.normal().Get(0).y(), 5e-2); + EXPECT_NEAR(1.0, contact.normal().Get(0).z(), 5e-2); + + } + } +} + TEST_F(ContactSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RemoveContactSensor)) { diff --git a/test/worlds/contact_extra_data.sdf b/test/worlds/contact_extra_data.sdf new file mode 100644 index 00000000000..def44661df4 --- /dev/null +++ b/test/worlds/contact_extra_data.sdf @@ -0,0 +1,105 @@ + + + + + 0 + + + + + + + + 0 0 -9.81 + + + true + + + + + 0 0 1 + 1 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 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + 0 0 0 0 -0 0 + false + + + + 0 0 0.5 0 0.0 0 + + 1.0 + + 0.4 + 0.4 + 0.4 + + + + + + + 1.0 + + + + + + + 1.0 + + + + + + collision_sphere + /test_extra_collision_data + + 1 + 1000 + + + + +