Skip to content

Commit

Permalink
Merge ad6b339 into f2ff574
Browse files Browse the repository at this point in the history
  • Loading branch information
@antbre
antbre authored Aug 3, 2023
2 parents f2ff574 + ad6b339 commit 3f4b8db
Show file tree
Hide file tree
Showing 3 changed files with 251 additions and 13 deletions.
66 changes: 53 additions & 13 deletions src/systems/physics/Physics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <utility>

#include <gz/common/geospatial/Dem.hh>
#include <gz/common/geospatial/HeightmapData.hh>
Expand Down Expand Up @@ -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<Policy>;

// A contact is described by a contactPoint and the corresponding
// extraContactData which we bundle in a pair data structure
using ContactData = std::pair<const WorldShapeType::ContactPoint *,
const ExtraContactData *>;
// 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<Entity,
std::deque<const WorldShapeType::ContactPoint *>>;
using EntityContactMap = std::unordered_map<Entity, std::deque<ContactData>>;

// 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
Expand All @@ -3761,16 +3771,19 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)

for (const auto &contactComposite : allContacts)
{
const auto &contact = contactComposite.Get<WorldShapeType::ContactPoint>();
auto coll1Entity =
this->entityCollisionMap.GetByPhysicsId(contact.collision1->EntityID());
auto coll2Entity =
this->entityCollisionMap.GetByPhysicsId(contact.collision2->EntityID());
const auto &contactPoint =
contactComposite.Get<WorldShapeType::ContactPoint>();
const auto &extraContactData = contactComposite.Query<ExtraContactData>();
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);
}
}

Expand Down Expand Up @@ -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);
}
}
}

Expand Down
93 changes: 93 additions & 0 deletions test/integration/contact_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<msgs::Contacts> contactMsgs;

auto contactCb = [&](const msgs::Contacts &_msg) -> void
{
std::lock_guard<std::mutex> 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<void(const msgs::Contacts &)>(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<std::mutex> lock(contactMutex);
ASSERT_GE(contactMsgs.size(), 1u);
}

{
std::lock_guard<std::mutex> 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))
{
Expand Down
105 changes: 105 additions & 0 deletions test/worlds/contact_extra_data.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="contact_extra_data">
<physics name="fast" type="ignored">
<real_time_factor>0</real_time_factor>
</physics>

<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-contact-system"
name="gz::sim::systems::Contact">
</plugin>

<gravity>0 0 -9.81</gravity>

<model name='ground_plane'>
<static>true</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>1 1</size>
</plane>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<bounce/>
<contact/>
</surface>
</collision>
<visual name='visual'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<pose>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<enable_wind>false</enable_wind>
</link>
<pose>0 0 0 0 -0 0</pose>
<self_collide>false</self_collide>
</model>

<model name="contact_model">
<pose>0 0 0.5 0 0.0 0</pose>
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.4</ixx>
<iyy>0.4</iyy>
<izz>0.4</izz>
</inertia>
</inertial>
<link name="link">
<collision name="collision_sphere">
<geometry>
<sphere>
<radius>1.0</radius>
</sphere>
</geometry>
</collision>
<visual name="visual_sphere">
<geometry>
<sphere>
<radius>1.0</radius>
</sphere>
</geometry>
</visual>
<sensor name='sensor_contact' type='contact'>
<contact>
<collision>collision_sphere</collision>
<topic>/test_extra_collision_data</topic>
</contact>
<always_on>1</always_on>
<update_rate>1000</update_rate>
</sensor>
</link>
</model>
</world>
</sdf>

0 comments on commit 3f4b8db

Please sign in to comment.