From dc04d03865c62ed7c38a8e6b13cbe4abaafcc1ec Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Tue, 24 Jan 2023 18:07:57 +0100 Subject: [PATCH 1/6] update all velocity and acceleration components of non-link entities fixes #1866 Signed-off-by: Alaa El Jawad --- src/systems/physics/Physics.cc | 114 +++++++++++++++++++++++++++++++++ 1 file changed, 114 insertions(+) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1a83e6a906..e1d52a8754 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3327,6 +3327,53 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); + + // body linear velocity + _ecm.Each( + [&](const Entity&, const components::Pose *_pose, + components::LinearVelocity *_linearVel, + const components::ParentEntity *_parent)->bool + { + // check if parent entity is a link, e.g. entity is sensor / collision + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); + math::Vector3d entityWorldLinearVel = + math::eigen3::convert(entityFrameData.linearVelocity); + + auto entityBodyLinearVel = + entityWorldPose.Rot().RotateVectorReverse(entityWorldLinearVel); + *_linearVel = components::LinearVelocity(entityBodyLinearVel); + } + + return true; + }); + + // world angular velocity + _ecm.Each( + [&](const Entity&, + const components::Pose *_pose, + components::WorldAngularVelocity *_worldAngularVel, + const components::ParentEntity *_parent)->bool + { + // check if parent entity is a link, e.g. entity is sensor / collision + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + // set entity world angular velocity + *_worldAngularVel = components::WorldAngularVelocity( + math::eigen3::convert(entityFrameData.angularVelocity)); + } + + return true; + }); // body angular velocity _ecm.Each( + [&](const Entity &, + const components::Pose *_pose, + components::WorldLinearAcceleration *_worldLinearAcc, + const components::ParentEntity *_parent)->bool + { + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + // set entity world linear acceleration + *_worldLinearAcc = components::WorldLinearAcceleration( + math::eigen3::convert(entityFrameData.linearAcceleration)); + } + + return true; + }); // body linear acceleration _ecm.Each( + [&](const Entity &, + const components::Pose *_pose, + components::WorldAngularAcceleration *_worldAngularAcc, + const components::ParentEntity *_parent)->bool + { + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + // set entity world angular acceleration + *_worldAngularAcc = components::WorldAngularAcceleration( + math::eigen3::convert(entityFrameData.angularAcceleration)); + } + + return true; + }); + + // body angular acceleration + _ecm.Each( + [&](const Entity &, + const components::Pose *_pose, + components::AngularAcceleration *_angularAcc, + const components::ParentEntity *_parent)->bool + { + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); + math::Vector3d entityWorldAngularAcc = + math::eigen3::convert(entityFrameData.angularAcceleration); + + auto entityBodyAngularAcc = + entityWorldPose.Rot().RotateVectorReverse(entityWorldAngularAcc); + *_angularAcc = components::AngularAcceleration(entityBodyAngularAcc); + } + return true; }); GZ_PROFILE_END(); From d66e0f3bf75e1abd36757b142bb8e62988b151fc Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Tue, 14 Feb 2023 16:57:17 +0100 Subject: [PATCH 2/6] Add unit test to check that non-link entities are sig Signed-off-by: Alaa El Jawad --- test/integration/physics_system.cc | 210 ++++++++++++++++++++++++++++ test/worlds/non_link_components.sdf | 83 +++++++++++ 2 files changed, 293 insertions(+) create mode 100644 test/worlds/non_link_components.sdf diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index bee99946f1..5c6e05dffb 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -38,11 +38,16 @@ #include #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" @@ -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" @@ -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 collisionPoses; + std::vector linVels, angVels, worldLinVels, worldAngVels; + std::vector linAccs, angAccs, worldLinAccs, worldAngAccs; + testSystem.OnPreUpdate( + [&iterations, &nIters, &halfIters, &collisionName, &collisionWithOffsetName]( + const UpdateInfo &_info, EntityComponentManager &_ecm) + { + _ecm.EachNew( + [&](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(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + + } + return true; + }); + + _ecm.Each( + [&](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(_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( + [&](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."; + } + +} diff --git a/test/worlds/non_link_components.sdf b/test/worlds/non_link_components.sdf new file mode 100644 index 0000000000..596213031a --- /dev/null +++ b/test/worlds/non_link_components.sdf @@ -0,0 +1,83 @@ + + + + 0 0 -10 + + 0.001 + 1 + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 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 2 0 0 0 + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + 0.1 0.2 0 0 1.570796327 0 + + + 0.01 0.01 0.01 + + + + + + + 0.1 0.1 0.1 + + + + + + + + From 3a8cf6bfd14d24ba147cdf5f64b0cb6f7715c006 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Sat, 4 Mar 2023 06:11:56 +0100 Subject: [PATCH 3/6] check that collision offset is taken into account in vel/acc update Signed-off-by: Alaa El Jawad --- test/integration/physics_system.cc | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 5c6e05dffb..47df3d84a2 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -2347,7 +2347,6 @@ TEST_F(PhysicsSystemFixture, parentLink.EnableVelocityChecks(_ecm); parentLink.EnableAccelerationChecks(_ecm); using namespace gz::math; - auto dt = std::chrono::duration_cast(_info.dt).count(); // increasing force downward auto force = -Vector3d::UnitZ * iterations / nIters; // increasing torque around z @@ -2423,6 +2422,8 @@ TEST_F(PhysicsSystemFixture, << "Model should be falling down."; } + double norm_lin_vel_yz, norm_world_lin_vel_xy; + double norm_lin_acc_yz, norm_world_lin_acc_xy; // the first part checks that: // - the collision poses are updated correctly // - linear velocities (world/local) are updated. @@ -2435,6 +2436,11 @@ TEST_F(PhysicsSystemFixture, // 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"; + // on the local(world) YZ(XY) plane the acceleration should be 0 + norm_lin_acc_yz = sqrt(linAccs[i].Y() * linAccs[i].Y() + linAccs[i].Z() * linAccs[i].Z()); + EXPECT_NEAR(norm_lin_acc_yz, 0, 1e-2) << "Local Linear acceleration on YZ-plane should be 0"; + norm_world_lin_acc_xy = sqrt(worldLinAccs[i].X() * worldLinAccs[i].X() + worldLinAccs[i].Y() * worldLinAccs[i].Y()); + EXPECT_NEAR(norm_world_lin_acc_xy , 0, 1e-2) << "World Linear acceleration on XY-plane should be 0"; // --- LIN VEL (WORLD, LOCAL) // linear velocity should keep increasing, both local and world @@ -2443,6 +2449,11 @@ TEST_F(PhysicsSystemFixture, // 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"; + // on the local(world) YZ(XY) plane the velocity should be 0 + norm_lin_vel_yz = sqrt(linVels[i].Y() * linVels[i].Y() + linVels[i].Z() * linVels[i].Z()); + EXPECT_NEAR(norm_lin_vel_yz, 0, 1e-2) << "Local Linear velocity on YZ-plane should be zero"; + norm_world_lin_vel_xy = sqrt(worldLinVels[i].X() * worldLinVels[i].X() + worldLinVels[i].Y() * worldLinVels[i].Y()); + EXPECT_NEAR(norm_world_lin_vel_xy , 0, 1e-2) << "World Linear acceleration on XY-plane should be zero"; // --- ANG ACC (WORLD, LOCAL) // angular acc is constant = 0 (both world and local) @@ -2463,6 +2474,16 @@ TEST_F(PhysicsSystemFixture, // --- 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."; + // on the local(world) YZ(XY) plane the acceleration norm should be positive + norm_lin_acc_yz = sqrt(linAccs[i].Y() * linAccs[i].Y() + linAccs[i].Z() * linAccs[i].Z()); + EXPECT_GT(norm_lin_acc_yz, 0) << "Local Linear acceleration on YZ-plane should be positive"; + norm_world_lin_acc_xy = sqrt(worldLinAccs[i].X() * worldLinAccs[i].X() + worldLinAccs[i].Y() * worldLinAccs[i].Y()); + EXPECT_GT(norm_world_lin_acc_xy , 0) << "World Linear acceleration on XY-plane should be positive"; + // on the local(world) YZ(XY) plane the velocity should be positive + norm_lin_vel_yz = sqrt(linVels[i].Y() * linVels[i].Y() + linVels[i].Z() * linVels[i].Z()); + EXPECT_GT(norm_lin_vel_yz, 0) << "Local Linear velocity on YZ-plane should be positive"; + norm_world_lin_vel_xy = sqrt(worldLinVels[i].X() * worldLinVels[i].X() + worldLinVels[i].Y() * worldLinVels[i].Y()); + EXPECT_GT(norm_world_lin_vel_xy , 0) << "World Linear acceleration on XY-plane should be positive"; // --- ANG ACC (WORLD, LOCAL) EXPECT_LT(angAccs[i-1].Length(), angAccs[i].Length()) << "Local Angular Acceleration should be increasing."; From 21f4d65e37fe80b98dc05910cfbcb3fe35b29847 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Sat, 4 Mar 2023 06:12:17 +0100 Subject: [PATCH 4/6] trim whitespaces with pre-commit hook Signed-off-by: Alaa El Jawad --- src/systems/physics/Physics.cc | 26 +++++++++++++------------- test/integration/physics_system.cc | 28 ++++++++++++++-------------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index e1d52a8754..ad653c3df0 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3327,7 +3327,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); - + // body linear velocity _ecm.Each( @@ -3340,19 +3340,19 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, { const auto entityFrameData = this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); - + auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); - math::Vector3d entityWorldLinearVel = + math::Vector3d entityWorldLinearVel = math::eigen3::convert(entityFrameData.linearVelocity); - - auto entityBodyLinearVel = + + auto entityBodyLinearVel = entityWorldPose.Rot().RotateVectorReverse(entityWorldLinearVel); *_linearVel = components::LinearVelocity(entityBodyLinearVel); } return true; }); - + // world angular velocity _ecm.Each( @@ -3364,14 +3364,14 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, // check if parent entity is a link, e.g. entity is sensor / collision if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) { - const auto entityFrameData = + const auto entityFrameData = this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); - + // set entity world angular velocity *_worldAngularVel = components::WorldAngularVelocity( math::eigen3::convert(entityFrameData.angularVelocity)); } - + return true; }); @@ -3400,7 +3400,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); - + // world linear acceleration _ecm.Each( @@ -3413,7 +3413,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, { const auto entityFrameData = this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); - + // set entity world linear acceleration *_worldLinearAcc = components::WorldLinearAcceleration( math::eigen3::convert(entityFrameData.linearAcceleration)); @@ -3446,7 +3446,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); - + // world angular acceleration _ecm.Each( @@ -3459,7 +3459,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, { const auto entityFrameData = this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); - + // set entity world angular acceleration *_worldAngularAcc = components::WorldAngularAcceleration( math::eigen3::convert(entityFrameData.angularAcceleration)); diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 47df3d84a2..70d65bb834 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -2276,10 +2276,10 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, /// 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, +TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(NonLinkComponentsAreUpdated)) { - + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + @@ -2319,7 +2319,7 @@ TEST_F(PhysicsSystemFixture, const components::Name *_name, const components::ParentEntity *_parentEntity)->bool { - // we only enable the velocity, acceleration components for + // we only enable the velocity, acceleration components for // the collision_with_offset if (_name->Data() == collisionWithOffsetName){ enableComponent(_ecm, _entity); @@ -2331,9 +2331,9 @@ TEST_F(PhysicsSystemFixture, enableComponent(_ecm, _entity); enableComponent(_ecm, _entity); enableComponent(_ecm, _entity); - + } - return true; + return true; }); _ecm.Each( @@ -2356,7 +2356,7 @@ TEST_F(PhysicsSystemFixture, return true; }); }); - + // save all the components history testSystem.OnPostUpdate( @@ -2375,7 +2375,7 @@ TEST_F(PhysicsSystemFixture, const components::AngularAcceleration *_angularAcc, const components::WorldLinearAcceleration *_worldLinearAcc, const components::WorldAngularAcceleration *_worldAngularAcc) -> bool - { + { EXPECT_TRUE(_name->Data() == collisionWithOffsetName); if (_name->Data() == collisionWithOffsetName) { @@ -2413,15 +2413,15 @@ TEST_F(PhysicsSystemFixture, 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()) + EXPECT_GT(collisionPoses[i-1].Pos().Z(), collisionPoses[i].Pos().Z()) << "Model should be falling down."; } - + double norm_lin_vel_yz, norm_world_lin_vel_xy; double norm_lin_acc_yz, norm_world_lin_acc_xy; // the first part checks that: @@ -2441,7 +2441,7 @@ TEST_F(PhysicsSystemFixture, EXPECT_NEAR(norm_lin_acc_yz, 0, 1e-2) << "Local Linear acceleration on YZ-plane should be 0"; norm_world_lin_acc_xy = sqrt(worldLinAccs[i].X() * worldLinAccs[i].X() + worldLinAccs[i].Y() * worldLinAccs[i].Y()); EXPECT_NEAR(norm_world_lin_acc_xy , 0, 1e-2) << "World Linear acceleration on XY-plane should be 0"; - + // --- 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"; @@ -2464,7 +2464,7 @@ TEST_F(PhysicsSystemFixture, 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 @@ -2484,7 +2484,7 @@ TEST_F(PhysicsSystemFixture, EXPECT_GT(norm_lin_vel_yz, 0) << "Local Linear velocity on YZ-plane should be positive"; norm_world_lin_vel_xy = sqrt(worldLinVels[i].X() * worldLinVels[i].X() + worldLinVels[i].Y() * worldLinVels[i].Y()); EXPECT_GT(norm_world_lin_vel_xy , 0) << "World Linear acceleration on XY-plane should be positive"; - + // --- 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."; @@ -2493,5 +2493,5 @@ TEST_F(PhysicsSystemFixture, 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."; } - + } From 214841ede8919c617f211165aa0b2cbf7e0cd7fb Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 7 Mar 2023 04:18:15 +0000 Subject: [PATCH 5/6] Fix style Signed-off-by: Addisu Z. Taddese --- test/integration/physics_system.cc | 240 +++++++++++++++++------------ 1 file changed, 140 insertions(+), 100 deletions(-) diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 70d65bb834..752d0a9aa5 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -2277,13 +2277,12 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, /// 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)) + GZ_UTILS_TEST_DISABLED_ON_WIN32(NonLinkComponentsAreUpdated)) { - ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/non_link_components.sdf"; + const auto sdfFile = + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/non_link_components.sdf"; sdf::Root root; root.Load(sdfFile); @@ -2311,70 +2310,84 @@ TEST_F(PhysicsSystemFixture, std::vector linVels, angVels, worldLinVels, worldAngVels; std::vector linAccs, angAccs, worldLinAccs, worldAngAccs; testSystem.OnPreUpdate( - [&iterations, &nIters, &halfIters, &collisionName, &collisionWithOffsetName]( - const UpdateInfo &_info, EntityComponentManager &_ecm) - { - _ecm.EachNew( - [&](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(_ecm, _entity); - enableComponent(_ecm, _entity); - enableComponent(_ecm, _entity); - enableComponent(_ecm, _entity); - enableComponent(_ecm, _entity); - enableComponent(_ecm, _entity); - enableComponent(_ecm, _entity); - enableComponent(_ecm, _entity); - enableComponent(_ecm, _entity); - - } - return true; - }); - - _ecm.Each( - [&](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; - // 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; - }); - }); + [&iterations, &nIters, &halfIters, &collisionName, + &collisionWithOffsetName](const UpdateInfo &_info, + EntityComponentManager &_ecm) + { + _ecm.EachNew( + [&](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(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, + _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, + _entity); + enableComponent(_ecm, + _entity); + } + return true; + }); + _ecm.Each( + [&](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; + // 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( - [&](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 + [&](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) @@ -2391,9 +2404,9 @@ TEST_F(PhysicsSystemFixture, } return true; }); - ++iterations; - return true; - }); + ++iterations; + return true; + }); server.AddSystem(testSystem.systemPtr); server.Run(true, nIters, false); @@ -2418,12 +2431,12 @@ TEST_F(PhysicsSystemFixture, // (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."; + EXPECT_GT(collisionPoses[i - 1].Pos().Z(), collisionPoses[i].Pos().Z()) + << "Model should be falling down."; } - double norm_lin_vel_yz, norm_world_lin_vel_xy; - double norm_lin_acc_yz, norm_world_lin_acc_xy; + double normLinVelYz, normWorldLinVelXy; + double normLinAccYz, normWorldLinAccXy; // the first part checks that: // - the collision poses are updated correctly // - linear velocities (world/local) are updated. @@ -2434,26 +2447,40 @@ TEST_F(PhysicsSystemFixture, 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"; + 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"; // on the local(world) YZ(XY) plane the acceleration should be 0 - norm_lin_acc_yz = sqrt(linAccs[i].Y() * linAccs[i].Y() + linAccs[i].Z() * linAccs[i].Z()); - EXPECT_NEAR(norm_lin_acc_yz, 0, 1e-2) << "Local Linear acceleration on YZ-plane should be 0"; - norm_world_lin_acc_xy = sqrt(worldLinAccs[i].X() * worldLinAccs[i].X() + worldLinAccs[i].Y() * worldLinAccs[i].Y()); - EXPECT_NEAR(norm_world_lin_acc_xy , 0, 1e-2) << "World Linear acceleration on XY-plane should be 0"; + normLinAccYz = + sqrt(linAccs[i].Y() * linAccs[i].Y() + linAccs[i].Z() * linAccs[i].Z()); + EXPECT_NEAR(normLinAccYz, 0, 1e-2) + << "Local Linear acceleration on YZ-plane should be 0"; + normWorldLinAccXy = sqrt(worldLinAccs[i].X() * worldLinAccs[i].X() + + worldLinAccs[i].Y() * worldLinAccs[i].Y()); + EXPECT_NEAR(normWorldLinAccXy, 0, 1e-2) + << "World Linear acceleration on XY-plane should be 0"; // --- 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"; + 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"; + 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"; // on the local(world) YZ(XY) plane the velocity should be 0 - norm_lin_vel_yz = sqrt(linVels[i].Y() * linVels[i].Y() + linVels[i].Z() * linVels[i].Z()); - EXPECT_NEAR(norm_lin_vel_yz, 0, 1e-2) << "Local Linear velocity on YZ-plane should be zero"; - norm_world_lin_vel_xy = sqrt(worldLinVels[i].X() * worldLinVels[i].X() + worldLinVels[i].Y() * worldLinVels[i].Y()); - EXPECT_NEAR(norm_world_lin_vel_xy , 0, 1e-2) << "World Linear acceleration on XY-plane should be zero"; + normLinVelYz = + sqrt(linVels[i].Y() * linVels[i].Y() + linVels[i].Z() * linVels[i].Z()); + EXPECT_NEAR(normLinVelYz, 0, 1e-2) + << "Local Linear velocity on YZ-plane should be zero"; + normWorldLinVelXy = sqrt(worldLinVels[i].X() * worldLinVels[i].X() + + worldLinVels[i].Y() * worldLinVels[i].Y()); + EXPECT_NEAR(normWorldLinVelXy, 0, 1e-2) + << "World Linear acceleration on XY-plane should be zero"; // --- ANG ACC (WORLD, LOCAL) // angular acc is constant = 0 (both world and local) @@ -2472,26 +2499,39 @@ TEST_F(PhysicsSystemFixture, 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."; + 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."; // on the local(world) YZ(XY) plane the acceleration norm should be positive - norm_lin_acc_yz = sqrt(linAccs[i].Y() * linAccs[i].Y() + linAccs[i].Z() * linAccs[i].Z()); - EXPECT_GT(norm_lin_acc_yz, 0) << "Local Linear acceleration on YZ-plane should be positive"; - norm_world_lin_acc_xy = sqrt(worldLinAccs[i].X() * worldLinAccs[i].X() + worldLinAccs[i].Y() * worldLinAccs[i].Y()); - EXPECT_GT(norm_world_lin_acc_xy , 0) << "World Linear acceleration on XY-plane should be positive"; + normLinAccYz = + sqrt(linAccs[i].Y() * linAccs[i].Y() + linAccs[i].Z() * linAccs[i].Z()); + EXPECT_GT(normLinAccYz, 0) + << "Local Linear acceleration on YZ-plane should be positive"; + normWorldLinAccXy = sqrt(worldLinAccs[i].X() * worldLinAccs[i].X() + + worldLinAccs[i].Y() * worldLinAccs[i].Y()); + EXPECT_GT(normWorldLinAccXy, 0) + << "World Linear acceleration on XY-plane should be positive"; // on the local(world) YZ(XY) plane the velocity should be positive - norm_lin_vel_yz = sqrt(linVels[i].Y() * linVels[i].Y() + linVels[i].Z() * linVels[i].Z()); - EXPECT_GT(norm_lin_vel_yz, 0) << "Local Linear velocity on YZ-plane should be positive"; - norm_world_lin_vel_xy = sqrt(worldLinVels[i].X() * worldLinVels[i].X() + worldLinVels[i].Y() * worldLinVels[i].Y()); - EXPECT_GT(norm_world_lin_vel_xy , 0) << "World Linear acceleration on XY-plane should be positive"; + normLinVelYz = + sqrt(linVels[i].Y() * linVels[i].Y() + linVels[i].Z() * linVels[i].Z()); + EXPECT_GT(normLinVelYz, 0) + << "Local Linear velocity on YZ-plane should be positive"; + normWorldLinVelXy = sqrt(worldLinVels[i].X() * worldLinVels[i].X() + + worldLinVels[i].Y() * worldLinVels[i].Y()); + EXPECT_GT(normWorldLinVelXy, 0) + << "World Linear acceleration on XY-plane should be positive"; // --- 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."; + 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."; + 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."; } - } From cb1213eb1fb3b451eea5fd064f7eda5c7f13b36e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 7 Mar 2023 16:38:19 +0000 Subject: [PATCH 6/6] Fix compiler and linter warnings Signed-off-by: Addisu Z. Taddese --- src/systems/physics/Physics.cc | 3 ++- test/integration/physics_system.cc | 20 ++++++++++++-------- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index ad653c3df0..cdda897950 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3680,7 +3680,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, // TODO(louise) Skip this if there are no collision features this->UpdateCollisions(_ecm); -} +} // NOLINT readability/fn_size +// TODO (azeey) Reduce size of function and remove the NOLINT above ////////////////////////////////////////////////// void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 752d0a9aa5..7bf469b557 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -2310,15 +2310,15 @@ TEST_F(PhysicsSystemFixture, std::vector linVels, angVels, worldLinVels, worldAngVels; std::vector linAccs, angAccs, worldLinAccs, worldAngAccs; testSystem.OnPreUpdate( - [&iterations, &nIters, &halfIters, &collisionName, - &collisionWithOffsetName](const UpdateInfo &_info, + [&iterations, &nIters, &halfIters, + &collisionWithOffsetName](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.EachNew( - [&](const Entity &_entity, const components::Collision *_collision, + [&](const Entity &_entity, const components::Collision *, const components::Name *_name, - const components::ParentEntity *_parentEntity) -> bool + const components::ParentEntity *) -> bool { // we only enable the velocity, acceleration components for // the collision_with_offset @@ -2342,7 +2342,7 @@ TEST_F(PhysicsSystemFixture, _ecm.Each( - [&](const Entity &_entity, const components::Collision *_collision, + [&](const Entity &, const components::Collision *, const components::Name *_name, const components::ParentEntity *_parentEntity) -> bool { @@ -2356,9 +2356,13 @@ TEST_F(PhysicsSystemFixture, parentLink.EnableAccelerationChecks(_ecm); using namespace gz::math; // increasing force downward - auto force = -Vector3d::UnitZ * iterations / nIters; + auto force = + -Vector3d::UnitZ * (static_cast(iterations) / + static_cast(nIters)); // increasing torque around z - auto torque = Vector3d::UnitZ * iterations / nIters; + auto torque = + Vector3d::UnitZ * (static_cast(iterations) / + static_cast(nIters)); parentLink.AddWorldWrench(_ecm, force, torque); } return true; @@ -2376,7 +2380,7 @@ TEST_F(PhysicsSystemFixture, components::LinearAcceleration, components::AngularAcceleration, components::WorldLinearAcceleration, components::WorldAngularAcceleration>( - [&](const Entity &, const components::Collision *_collision, + [&](const Entity &, const components::Collision *, const components::Name *_name, const components::WorldPose *_pose, const components::LinearVelocity *_linearVel,