Skip to content

Commit

Permalink
[bullet-featherstone] Ignore collision between static objects and obj…
Browse files Browse the repository at this point in the history 
…ects with world joint (#611)

* ignore collision between static link and base link with world joint

Signed-off-by: Ian Chen <[email protected]>

* check link with zero dof

Signed-off-by: Ian Chen <[email protected]>

* update test

Signed-off-by: Ian Chen <[email protected]>

---------

Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
@iche033
iche033 authored Apr 4, 2024
1 parent d616379 commit 2bc19ad
Show file tree
Hide file tree
Showing 2 changed files with 143 additions and 1 deletion.
19 changes: 18 additions & 1 deletion bullet-featherstone/src/SDFFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1205,7 +1205,24 @@ bool SDFFeatures::AddSdfCollision(

auto *world = this->ReferenceInterface<WorldInfo>(model->world);

if (isStatic)
// Set static filter for collisions in
// 1) a static model
// 2) a fixed base link
// 3) a (non-base) link with zero dofs
bool isFixed = false;
if (model->body->hasFixedBase())
{
// check if it's a base link
isFixed = std::size_t(_linkID) ==
static_cast<std::size_t>(model->body->getUserIndex());
// check if link has zero dofs
if (!isFixed && linkInfo->indexInModel.has_value())
{
isFixed = model->body->getLink(
linkInfo->indexInModel.value()).m_dofCount == 0;
}
}
if (isStatic || isFixed)
{
world->world->addCollisionObject(
linkInfo->collider.get(),
Expand Down
125 changes: 125 additions & 0 deletions test/common_test/collisions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,28 +16,34 @@
*/
#include <gtest/gtest.h>

#include <sstream>
#include <string>

#include <gz/common/Console.hh>
#include <gz/plugin/Loader.hh>

#include "test/Resources.hh"
#include "test/TestLibLoader.hh"
#include "Worlds.hh"

#include <gz/physics/FindFeatures.hh>
#include <gz/physics/RequestEngine.hh>
#include <gz/physics/ConstructEmpty.hh>
#include <gz/physics/ForwardStep.hh>
#include <gz/physics/FrameSemantics.hh>
#include <gz/physics/FreeJoint.hh>
#include <gz/physics/GetContacts.hh>
#include <gz/physics/GetEntities.hh>
#include <gz/physics/mesh/MeshShape.hh>
#include <gz/physics/PlaneShape.hh>
#include <gz/physics/FixedJoint.hh>
#include <gz/physics/sdf/ConstructModel.hh>
#include <gz/physics/sdf/ConstructWorld.hh>

#include <gz/common/MeshManager.hh>

#include <sdf/Root.hh>

template <class T>
class CollisionTest:
public testing::Test, public gz::physics::TestLibLoader
Expand Down Expand Up @@ -141,6 +147,125 @@ TYPED_TEST(CollisionTest, MeshAndPlane)
}
}

using CollisionStaticFeaturesList = gz::physics::FeatureList<
gz::physics::sdf::ConstructSdfModel,
gz::physics::sdf::ConstructSdfWorld,
gz::physics::GetContactsFromLastStepFeature,
gz::physics::ForwardStep
>;

using CollisionStaticTestFeaturesList =
CollisionTest<CollisionStaticFeaturesList>;

TEST_F(CollisionStaticTestFeaturesList, StaticCollisions)
{
auto getBoxStaticStr = [](const std::string &_name,
const gz::math::Pose3d &_pose)
{
std::stringstream modelStaticStr;
modelStaticStr << R"(
<sdf version="1.11">
<model name=")";
modelStaticStr << _name << R"(">
<pose>)";
modelStaticStr << _pose;
modelStaticStr << R"(</pose>
<link name="body">
<collision name="collision">
<geometry>
<box><size>1 1 1</size></box>
</geometry>
</collision>
</link>
<static>true</static>
</model>
</sdf>)";
return modelStaticStr.str();
};

auto getBoxFixedJointStr = [](const std::string &_name,
const gz::math::Pose3d &_pose)
{
std::stringstream modelFixedJointStr;
modelFixedJointStr << R"(
<sdf version="1.11">
<model name=")";
modelFixedJointStr << _name << R"(">
<pose>)";
modelFixedJointStr << _pose;
modelFixedJointStr << R"(</pose>
<link name="body">
<collision name="collision">
<geometry>
<box><size>1 1 1</size></box>
</geometry>
</collision>
</link>
<joint name="world_fixed" type="fixed">
<parent>world</parent>
<child>body</child>
</joint>
</model>
</sdf>)";
return modelFixedJointStr.str();
};

for (const std::string &name : this->pluginNames)
{
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<CollisionStaticFeaturesList>::From(plugin);
ASSERT_NE(nullptr, engine);

auto world = engine->ConstructWorld(*rootWorld.WorldByIndex(0));
ASSERT_NE(nullptr, world);

sdf::Root root;
sdf::Errors errors = root.LoadSdfString(getBoxStaticStr(
"box_static", gz::math::Pose3d::Zero));
ASSERT_TRUE(errors.empty()) << errors.front();
ASSERT_NE(nullptr, root.Model());
world->ConstructModel(*root.Model());

gz::physics::ForwardStep::Output output;
gz::physics::ForwardStep::State state;
gz::physics::ForwardStep::Input input;
for (std::size_t i = 0; i < 10; ++i)
{
world->Step(output, state, input);
}

// static box overlaps with ground plane
// verify no contacts between static bodies.
auto contacts = world->GetContactsFromLastStep();
EXPECT_EQ(0u, contacts.size());

// currently only bullet-featherstone skips collision checking between
// static bodies and bodies with world fixed joint
if (this->PhysicsEngineName(name) != "bullet-featherstone")
continue;

errors = root.LoadSdfString(getBoxFixedJointStr(
"box_fixed_world_joint", gz::math::Pose3d::Zero));
ASSERT_TRUE(errors.empty()) << errors.front();
ASSERT_NE(nullptr, root.Model());
world->ConstructModel(*root.Model());

world->Step(output, state, input);
// box fixed to world overlaps with static box and ground plane
// verify there are still no contacts.
contacts = world->GetContactsFromLastStep();
EXPECT_EQ(0u, contacts.size());
}
}

int main(int argc, char *argv[])
{
::testing::InitGoogleTest(&argc, argv);
Expand Down

0 comments on commit 2bc19ad

Please sign in to comment.