Skip to content

Commit

Permalink
added tests
Browse files Browse the repository at this point in the history 
Signed-off-by: Ashton Larkin <[email protected]>
  • Loading branch information
@adlarkin
adlarkin committed Mar 15, 2021
1 parent 2eb7667 commit e4dea17
Show file tree
Hide file tree
Showing 2 changed files with 72 additions and 17 deletions.
43 changes: 35 additions & 8 deletions dartsim/src/SimulationFeatures_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,16 +92,37 @@ std::unordered_set<TestWorldPtr> LoadWorlds(
return worlds;
}

void StepWorld(const TestWorldPtr &_world, const std::size_t _num_steps = 1)
/// \brief Step forward in a world
/// \param[in] _world The world to step in
/// \param[in] _firstTime Whether this is the very first time this world is
/// being stepped in (true) or not (false)
/// \param[in] _numSteps The number of steps to take in _world
/// \return true if the forward step output was checked, false otherwise
bool StepWorld(const TestWorldPtr &_world, bool _firstTime,
const std::size_t _numSteps = 1)
{
ignition::physics::ForwardStep::Input input;
ignition::physics::ForwardStep::State state;
ignition::physics::ForwardStep::Output output;

for (size_t i = 0; i < _num_steps; ++i)
bool checkedOutput = false;
for (size_t i = 0; i < _numSteps; ++i)
{
_world->Step(output, state, input);

// If link poses have changed, this should have been written to output.
// Link poses are considered "changed" if they are new, so if this is the
// very first step in a world, all of the link data is new and output
// should not be empty
if (_firstTime && (i == 0))
{
EXPECT_FALSE(
output.Get<ignition::physics::WorldPoses>().entries.empty());
checkedOutput = true;
}
}

return checkedOutput;
}

class SimulationFeatures_TEST
Expand All @@ -121,7 +142,8 @@ TEST_P(SimulationFeatures_TEST, Falling)

for (const auto &world : worlds)
{
StepWorld(world, 1000);
auto checkedOutput =StepWorld(world, true, 1000);
EXPECT_TRUE(checkedOutput);

auto link = world->GetModel(0)->GetLink(0);
auto pos = link->FrameDataRelativeToWorld().pose.translation();
Expand Down Expand Up @@ -195,7 +217,8 @@ TEST_P(SimulationFeatures_TEST, CollideBitmasks)
auto filteredBox = world->GetModel("box_filtered");
auto collidingBox = world->GetModel("box_colliding");

StepWorld(world);
auto checkedOutput = StepWorld(world, true);
EXPECT_TRUE(checkedOutput);
auto contacts = world->GetContactsFromLastStep();
// Only one box (box_colliding) should collide
EXPECT_EQ(4u, contacts.size());
Expand All @@ -207,15 +230,17 @@ TEST_P(SimulationFeatures_TEST, CollideBitmasks)
// Also test the getter
EXPECT_EQ(0xF0, collidingShape->GetCollisionFilterMask());
// Step and make sure there is no collisions
StepWorld(world);
checkedOutput = StepWorld(world, false);
EXPECT_FALSE(checkedOutput);
contacts = world->GetContactsFromLastStep();
EXPECT_EQ(0u, contacts.size());

// Now remove both filter masks (no collision will be filtered)
// Equivalent to set to 0xFF
collidingShape->RemoveCollisionFilterMask();
filteredShape->RemoveCollisionFilterMask();
StepWorld(world);
checkedOutput = StepWorld(world, false);
EXPECT_FALSE(checkedOutput);
// Expect both objects to collide
contacts = world->GetContactsFromLastStep();
EXPECT_EQ(8u, contacts.size());
Expand All @@ -240,10 +265,12 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
// The first step already has contacts, but the contact force due to the
// impact does not match the steady-state force generated by the
// body's weight.
StepWorld(world);
auto checkedOutput = StepWorld(world, true);
EXPECT_TRUE(checkedOutput);

// After a second step, the contact force reaches steady-state
StepWorld(world);
checkedOutput = StepWorld(world, false);
EXPECT_FALSE(checkedOutput);

auto contacts = world->GetContactsFromLastStep();
EXPECT_EQ(4u, contacts.size());
Expand Down
46 changes: 37 additions & 9 deletions tpe/plugin/src/SimulationFeatures_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,16 +90,37 @@ std::unordered_set<TestWorldPtr> LoadWorlds(
return worlds;
}

void StepWorld(const TestWorldPtr &_world, const std::size_t _num_steps = 1)
/// \brief Step forward in a world
/// \param[in] _world The world to step in
/// \param[in] _firstTime Whether this is the very first time this world is
/// being stepped in (true) or not (false)
/// \param[in] _numSteps The number of steps to take in _world
/// \return true if the forward step output was checked, false otherwise
bool StepWorld(const TestWorldPtr &_world, bool _firstTime,
const std::size_t _numSteps = 1)
{
ignition::physics::ForwardStep::Input input;
ignition::physics::ForwardStep::State state;
ignition::physics::ForwardStep::Output output;

for (size_t i = 0; i < _num_steps; ++i)
bool checkedOutput = false;
for (size_t i = 0; i < _numSteps; ++i)
{
_world->Step(output, state, input);

// If link poses have changed, this should have been written to output.
// Link poses are considered "changed" if they are new, so if this is the
// very first step in a world, all of the link data is new and output
// should not be empty
if (_firstTime && (i == 0))
{
EXPECT_FALSE(
output.Get<ignition::physics::WorldPoses>().entries.empty());
checkedOutput = true;
}
}

return checkedOutput;
}

class SimulationFeatures_TEST
Expand All @@ -119,7 +140,8 @@ TEST_P(SimulationFeatures_TEST, StepWorld)

for (const auto &world : worlds)
{
StepWorld(world, 1000);
auto checkedOutput = StepWorld(world, true, 1000);
EXPECT_TRUE(checkedOutput);

auto model = world->GetModel(0);
ASSERT_NE(nullptr, model);
Expand Down Expand Up @@ -269,7 +291,8 @@ TEST_P(SimulationFeatures_TEST, CollideBitmasks)
auto filteredBox = world->GetModel("box_filtered");
auto collidingBox = world->GetModel("box_colliding");

StepWorld(world);
auto checkedOutput = StepWorld(world, true);
EXPECT_TRUE(checkedOutput);
auto contacts = world->GetContactsFromLastStep();
// Only box_colliding should collide with box_base
EXPECT_EQ(1u, contacts.size());
Expand All @@ -281,15 +304,17 @@ TEST_P(SimulationFeatures_TEST, CollideBitmasks)
// Also test the getter
EXPECT_EQ(0xF0, collidingShape->GetCollisionFilterMask());
// Step and make sure there are no collisions
StepWorld(world);
checkedOutput = StepWorld(world, false);
EXPECT_FALSE(checkedOutput);
contacts = world->GetContactsFromLastStep();
EXPECT_EQ(0u, contacts.size());

// Now remove both filter masks (no collisions will be filtered)
// Equivalent to 0xFF
collidingShape->RemoveCollisionFilterMask();
filteredShape->RemoveCollisionFilterMask();
StepWorld(world);
checkedOutput = StepWorld(world, false);
EXPECT_FALSE(checkedOutput);
// Expect box_filtered and box_colliding to collide with box_base
contacts = world->GetContactsFromLastStep();
EXPECT_EQ(2u, contacts.size());
Expand Down Expand Up @@ -317,7 +342,8 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
auto box = world->GetModel("box");

// step and get contacts
StepWorld(world, 1);
auto checkedOutput = StepWorld(world, true);
EXPECT_TRUE(checkedOutput);
auto contacts = world->GetContactsFromLastStep();

// large box in the middle should be intersecting with sphere and cylinder
Expand Down Expand Up @@ -366,7 +392,8 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
ignition::math::Pose3d(0, 100, 0.5, 0, 0, 0)));

// step and get contacts
StepWorld(world, 1);
checkedOutput = StepWorld(world, false);
EXPECT_FALSE(checkedOutput);
contacts = world->GetContactsFromLastStep();

// large box in the middle should be intersecting with cylinder
Expand Down Expand Up @@ -401,7 +428,8 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
ignition::math::Pose3d(0, -100, 0.5, 0, 0, 0)));

// step and get contacts
StepWorld(world, 1);
checkedOutput = StepWorld(world, false);
EXPECT_FALSE(checkedOutput);
contacts = world->GetContactsFromLastStep();

// no entities should be colliding
Expand Down

0 comments on commit e4dea17

Please sign in to comment.