Skip to content

Commit

Permalink
Add unit test to check that non-link entities are sig
Browse files Browse the repository at this point in the history 
Signed-off-by: Alaa El Jawad <[email protected]>
  • Loading branch information
@ejalaa12
ejalaa12 committed Feb 14, 2023
1 parent fd6a1e6 commit 7169856
Show file tree
Hide file tree
Showing 2 changed files with 293 additions and 0 deletions.
210 changes: 210 additions & 0 deletions test/integration/physics_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,16 @@
#include <sdf/World.hh>

#include "gz/sim/Entity.hh"
#include "gz/sim/EntityComponentManager.hh"
#include "gz/sim/Link.hh"
#include "gz/sim/Server.hh"
#include "gz/sim/SystemLoader.hh"
#include "gz/sim/Types.hh"
#include "gz/sim/Util.hh"
#include "test_config.hh" // NOLINT(build/include)

#include "gz/sim/components/AngularAcceleration.hh"
#include "gz/sim/components/AngularVelocity.hh"
#include "gz/sim/components/AxisAlignedBox.hh"
#include "gz/sim/components/CanonicalLink.hh"
#include "gz/sim/components/Collision.hh"
Expand All @@ -60,6 +65,7 @@
#include "gz/sim/components/JointVelocityLimitsCmd.hh"
#include "gz/sim/components/JointVelocityReset.hh"
#include "gz/sim/components/Link.hh"
#include "gz/sim/components/LinearAcceleration.hh"
#include "gz/sim/components/LinearVelocity.hh"
#include "gz/sim/components/Material.hh"
#include "gz/sim/components/Model.hh"
Expand Down Expand Up @@ -2264,3 +2270,207 @@ TEST_F(PhysicsSystemFixtureWithDart6_10,
server->AddSystem(testSystem.systemPtr);
server->Run(true, nIters, false);
}


//////////////////////////////////////////////////
/// This test makes sure that non-link entities' components
/// are updated by the physics system if they have been enabled.
/// A collision is a non-link entity
TEST_F(PhysicsSystemFixture,
GZ_UTILS_TEST_DISABLED_ON_WIN32(NonLinkComponentsAreUpdated))
{

ServerConfig serverConfig;

const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/non_link_components.sdf";

sdf::Root root;
root.Load(sdfFile);
const sdf::World *world = root.WorldByIndex(0);
auto gravity = world->Gravity();
ASSERT_TRUE(nullptr != world);

serverConfig.SetSdfFile(sdfFile);

Server server(serverConfig);

server.SetUpdatePeriod(1ms);

const std::string collisionName{"collision"};
const std::string collisionWithOffsetName{"collision_with_offset"};

// Create a system that records the pose, vel, acc of the collisions.
test::Relay testSystem;

size_t iterations = 0u;
std::size_t nIters{100};
std::size_t halfIters{nIters / 2};

std::vector<math::Pose3d> collisionPoses;
std::vector<math::Vector3d> linVels, angVels, worldLinVels, worldAngVels;
std::vector<math::Vector3d> linAccs, angAccs, worldLinAccs, worldAngAccs;
testSystem.OnPreUpdate(
[&iterations, &nIters, &halfIters, &collisionName, &collisionWithOffsetName](
const UpdateInfo &_info, EntityComponentManager &_ecm)
{
_ecm.EachNew<components::Collision, components::Name, components::ParentEntity>(
[&](const Entity &_entity, const components::Collision *_collision,
const components::Name *_name,
const components::ParentEntity *_parentEntity)->bool
{
// we only enable the velocity, acceleration components for
// the collision_with_offset
if (_name->Data() == collisionWithOffsetName){
enableComponent<components::WorldPose>(_ecm, _entity);
enableComponent<components::LinearVelocity>(_ecm, _entity);
enableComponent<components::AngularVelocity>(_ecm, _entity);
enableComponent<components::WorldLinearVelocity>(_ecm, _entity);
enableComponent<components::WorldAngularVelocity>(_ecm, _entity);
enableComponent<components::LinearAcceleration>(_ecm, _entity);
enableComponent<components::AngularAcceleration>(_ecm, _entity);
enableComponent<components::WorldLinearAcceleration>(_ecm, _entity);
enableComponent<components::WorldAngularAcceleration>(_ecm, _entity);

}
return true;
});

_ecm.Each<components::Collision, components::Name, components::ParentEntity>(
[&](const Entity &_entity, const components::Collision *_collision,
const components::Name *_name,
const components::ParentEntity *_parentEntity)->bool
{
// after half of the simulation, apply a varying wrench, on the link
if (_name->Data() == collisionWithOffsetName && iterations >= halfIters){
gz::sim::Link parentLink(_parentEntity->Data());
parentLink.EnableVelocityChecks(_ecm);
parentLink.EnableAccelerationChecks(_ecm);
using namespace gz::math;
auto dt = std::chrono::duration_cast<std::chrono::nanoseconds>(_info.dt).count();
// increasing force downward
auto force = -Vector3d::UnitZ * iterations / nIters;
// increasing torque around z
auto torque = Vector3d::UnitZ * iterations / nIters;
parentLink.AddWorldWrench(_ecm, force, torque);
}
return true;
});
});


// save all the components history
testSystem.OnPostUpdate(
[&](const UpdateInfo &, const EntityComponentManager &_ecm) {
_ecm.Each<components::Collision, components::Name, components::WorldPose,
components::LinearVelocity, components::AngularVelocity,
components::WorldLinearVelocity, components::WorldAngularVelocity,
components::LinearAcceleration, components::AngularAcceleration,
components::WorldLinearAcceleration,
components::WorldAngularAcceleration>(
[&](const Entity &, const components::Collision *_collision,
const components::Name *_name, const components::WorldPose *_pose,
const components::LinearVelocity *_linearVel, const components::AngularVelocity *_angularVel,
const components::WorldLinearVelocity *_worldLinearVel,
const components::WorldAngularVelocity *_worldAngularVel, const components::LinearAcceleration *_linearAcc,
const components::AngularAcceleration *_angularAcc,
const components::WorldLinearAcceleration *_worldLinearAcc,
const components::WorldAngularAcceleration *_worldAngularAcc) -> bool
{
EXPECT_TRUE(_name->Data() == collisionWithOffsetName);
if (_name->Data() == collisionWithOffsetName)
{
collisionPoses.push_back(_pose->Data());
linVels.push_back(_linearVel->Data());
angVels.push_back(_angularVel->Data());
worldLinVels.push_back(_worldLinearVel->Data());
worldAngVels.push_back(_worldAngularVel->Data());
linAccs.push_back(_linearAcc->Data());
angAccs.push_back(_angularAcc->Data());
worldLinAccs.push_back(_worldLinearAcc->Data());
worldAngAccs.push_back(_worldAngularAcc->Data());
}
return true;
});
++iterations;
return true;
});

server.AddSystem(testSystem.systemPtr);
server.Run(true, nIters, false);

ASSERT_EQ(nIters, collisionPoses.size());
ASSERT_EQ(nIters, linVels.size());
ASSERT_EQ(nIters, angVels.size());
ASSERT_EQ(nIters, worldLinVels.size());
ASSERT_EQ(nIters, worldAngVels.size());
ASSERT_EQ(nIters, linAccs.size());
ASSERT_EQ(nIters, angAccs.size());
ASSERT_EQ(nIters, worldLinAccs.size());
ASSERT_EQ(nIters, worldAngAccs.size());
ASSERT_EQ(nIters, iterations);

// The collision offset should be taken into account in the world pose
EXPECT_NEAR(0.1, collisionPoses.front().Pos().X(), 1e-2);
EXPECT_NEAR(0.2, collisionPoses.front().Pos().Y(), 1e-2);
EXPECT_NEAR(2, collisionPoses.front().Pos().Z(), 1e-2);

// Make sure the position is updated every time step
// (here, the model is falling down, so only z should be updated)
for (size_t i = 1; i <= nIters; i++)
{
EXPECT_GT(collisionPoses[i-1].Pos().Z(), collisionPoses[i].Pos().Z())
<< "Model should be falling down.";
}

// the first part checks that:
// - the collision poses are updated correctly
// - linear velocities (world/local) are updated.
for (size_t i = 1; i < halfIters; i++)
{
// --- LIN ACC (WORLD, LOCAL)
// linear acc is constant = gravity (both world and local)
EXPECT_NEAR(linAccs[i].Length(), gravity.Length(), 1e-2);
EXPECT_NEAR(worldLinAccs[i].Length(), gravity.Length(), 1e-2);
// local should mostly be X , world should be -Z
EXPECT_NEAR(linAccs[i].X(), gravity.Length(), 1e-2) << "Local linear acceleration should be along X axis";
EXPECT_NEAR(worldLinAccs[i].Z(), -gravity.Length(), 1e-2) << "World Linear acceleration should be along -Z axis";

// --- LIN VEL (WORLD, LOCAL)
// linear velocity should keep increasing, both local and world
EXPECT_LT(linVels[i-1].Length(), linVels[i].Length()) << "Local linear velocity should keep increasing";
EXPECT_LT(worldLinVels[i-1].Length(), worldLinVels[i].Length()) << "World linear velocity should keep increasing";
// local should mostly be X , world should be -Z
EXPECT_GT(linVels[i-1].X(), 0) << "Local linear vel should be positive in x";
EXPECT_LT(worldLinVels[i-1].Z(), 0) << "World linear vel should be negative in z";

// --- ANG ACC (WORLD, LOCAL)
// angular acc is constant = 0 (both world and local)
EXPECT_NEAR(angAccs[i].Length(), 0, 1e-2);
EXPECT_NEAR(worldAngAccs[i].Length(), 0, 1e-2);
// --- ANG VEL (WORLD, LOCAL)
// angular velocity is constant
EXPECT_NEAR(angVels[i].Length(), 0, 1e-2);
EXPECT_NEAR(worldAngVels[i].Length(), 0, 1e-2);
}

// Second part, we applied a varying wrench, and we should see:
// - linear acceleration (world/local) are updated
// - angular acceleration (world/local) are updated
// - angular velocities (world/local) are updated
for (size_t i = halfIters + 1; i < nIters; i++)
{
// --- LIN ACC (WORLD, LOCAL)
EXPECT_LT(linAccs[i-1].Length(), linAccs[i].Length()) << "Local Linear Acceleration should be increasing.";
EXPECT_LT(worldLinAccs[i-1].Length(), worldLinAccs[i].Length()) << "World Linear Acceleration should be increasing.";

// --- ANG ACC (WORLD, LOCAL)
EXPECT_LT(angAccs[i-1].Length(), angAccs[i].Length()) << "Local Angular Acceleration should be increasing.";
EXPECT_LT(worldAngAccs[i-1].Length(), worldAngAccs[i].Length()) << "World Angular Acceleration should be increasing.";

// --- ANG VEL (WORLD, LOCAL)
EXPECT_LT(angVels[i-1].Length(), angVels[i].Length()) << "Local Angular Velocity should be increasing.";
EXPECT_LT(worldAngVels[i-1].Length(), worldAngVels[i].Length()) << "World Angular Velocity should be increasing.";
}

}
83 changes: 83 additions & 0 deletions test/worlds/non_link_components.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="non_link_component_world">
<gravity>0 0 -10</gravity>
<physics name="1ms" type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</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>
</link>
</model>

<model name="model_with_non_link_entity">
<pose>0 0 2 0 0 0</pose>
<link name="link">
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000166667</ixx>
<iyy>0.000166667</iyy>
<izz>0.000166667</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<collision name="collision_with_offset">
<!-- rotate collision for testing purposes -->
<pose>0.1 0.2 0 0 1.570796327 0</pose>
<geometry>
<box>
<size>0.01 0.01 0.01</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
</link>
</model>

</world>
</sdf>

0 comments on commit 7169856

Please sign in to comment.