From 43b7957efb64505cb54e53498e85366dc7213c87 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 28 Nov 2022 16:44:13 -0800 Subject: [PATCH] Reduce more use of ignition:: Signed-off-by: Nate Koenig --- dartsim/src/Collisions_TEST.cc | 52 +++-- dartsim/src/CustomMeshShape.cc | 16 +- dartsim/src/CustomMeshShape.hh | 4 +- dartsim/src/EntityManagement_TEST.cc | 40 ++-- dartsim/src/LinkFeatures_TEST.cc | 20 +- dartsim/src/SDFFeatures.cc | 10 +- dartsim/src/SDFFeatures_TEST.cc | 52 +++-- dartsim/src/ShapeFeatures.cc | 2 +- dartsim/src/ShapeFeatures.hh | 2 +- dartsim/src/SimulationFeatures_TEST.cc | 82 +++---- tpe/lib/src/Shape.hh | 2 +- tpe/plugin/src/EntityManagement_TEST.cc | 18 +- tpe/plugin/src/SDFFeatures_TEST.cc | 270 +++++++++++----------- tpe/plugin/src/SimulationFeatures_TEST.cc | 128 +++++----- 14 files changed, 357 insertions(+), 341 deletions(-) diff --git a/dartsim/src/Collisions_TEST.cc b/dartsim/src/Collisions_TEST.cc index e6b383a8b..c8f572297 100644 --- a/dartsim/src/Collisions_TEST.cc +++ b/dartsim/src/Collisions_TEST.cc @@ -35,21 +35,23 @@ #include -using TestFeatureList = ignition::physics::FeatureList< - ignition::physics::LinkFrameSemantics, - ignition::physics::ForwardStep, - ignition::physics::GetEntities, - ignition::physics::ConstructEmptyWorldFeature, - ignition::physics::ConstructEmptyModelFeature, - ignition::physics::ConstructEmptyLinkFeature, - ignition::physics::mesh::AttachMeshShapeFeature, - ignition::physics::AttachPlaneShapeFeature, - ignition::physics::SetFreeJointRelativeTransformFeature, - ignition::physics::AttachFixedJointFeature +using namespace ignition; + +using TestFeatureList = physics::FeatureList< + physics::LinkFrameSemantics, + physics::ForwardStep, + physics::GetEntities, + physics::ConstructEmptyWorldFeature, + physics::ConstructEmptyModelFeature, + physics::ConstructEmptyLinkFeature, + physics::mesh::AttachMeshShapeFeature, + physics::AttachPlaneShapeFeature, + physics::SetFreeJointRelativeTransformFeature, + physics::AttachFixedJointFeature >; -using TestWorldPtr = ignition::physics::World3dPtr; -using TestEnginePtr = ignition::physics::Engine3dPtr; +using TestWorldPtr = physics::World3dPtr; +using TestEnginePtr = physics::Engine3dPtr; using WorldConstructor = std::function; @@ -57,21 +59,21 @@ std::unordered_set LoadWorlds( const std::string &_library, const WorldConstructor &_constructor) { - ignition::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(_library); const std::set pluginNames = - ignition::physics::FindFeatures3d::From(loader); + physics::FindFeatures3d::From(loader); std::unordered_set worlds; for (const std::string &name : pluginNames) { - ignition::plugin::PluginPtr plugin = loader.Instantiate(name); + plugin::PluginPtr plugin = loader.Instantiate(name); std::cout << " -- Plugin name: " << name << std::endl; auto engine = - ignition::physics::RequestEngine3d::From(plugin); + physics::RequestEngine3d::From(plugin); EXPECT_NE(nullptr, engine); worlds.insert(_constructor(engine)); @@ -86,11 +88,11 @@ class Collisions_TEST {}; INSTANTIATE_TEST_CASE_P(PhysicsPlugins, Collisions_TEST, - ::testing::ValuesIn(ignition::physics::test::g_PhysicsPluginLibraries),); // NOLINT + ::testing::ValuesIn(physics::test::g_PhysicsPluginLibraries),); // NOLINT TestWorldPtr ConstructMeshPlaneWorld( - const ignition::physics::Engine3dPtr &_engine, - const ignition::common::Mesh &_mesh) + const physics::Engine3dPtr &_engine, + const common::Mesh &_mesh) { auto world = _engine->ConstructEmptyWorld("world"); @@ -109,7 +111,7 @@ TestWorldPtr ConstructMeshPlaneWorld( model = world->ConstructEmptyModel("plane"); link = model->ConstructEmptyLink("link"); - link->AttachPlaneShape("plane", ignition::physics::LinearVector3d::UnitZ()); + link->AttachPlaneShape("plane", physics::LinearVector3d::UnitZ()); link->AttachFixedJoint(nullptr); return world; @@ -122,7 +124,7 @@ TEST_P(Collisions_TEST, MeshAndPlane) return; const std::string meshFilename = IGNITION_PHYSICS_RESOURCE_DIR "/chassis.dae"; - auto &meshManager = *ignition::common::MeshManager::Instance(); + auto &meshManager = *common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); std::cout << "Testing library " << library << std::endl; @@ -138,9 +140,9 @@ TEST_P(Collisions_TEST, MeshAndPlane) EXPECT_NEAR( 0.0, link->FrameDataRelativeToWorld().pose.translation()[2], 1e-6); - ignition::physics::ForwardStep::Output output; - ignition::physics::ForwardStep::State state; - ignition::physics::ForwardStep::Input input; + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; for (std::size_t i = 0; i < 1000; ++i) { world->Step(output, state, input); diff --git a/dartsim/src/CustomMeshShape.cc b/dartsim/src/CustomMeshShape.cc index 2671efe22..eb5c89ef5 100644 --- a/dartsim/src/CustomMeshShape.cc +++ b/dartsim/src/CustomMeshShape.cc @@ -30,11 +30,11 @@ namespace dartsim { namespace { ///////////////////////////////////////////////// unsigned int CheckNumVerticesPerFaces( - const ignition::common::SubMesh &_inputSubmesh, + const common::SubMesh &_inputSubmesh, const unsigned int _submeshIndex, const std::string &_path) { - using namespace ignition::common; + using namespace common; const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); @@ -76,9 +76,9 @@ unsigned int CheckNumVerticesPerFaces( ///////////////////////////////////////////////// unsigned int GetPrimitiveType( - const ignition::common::SubMesh &_inputSubmesh) + const common::SubMesh &_inputSubmesh) { - using namespace ignition::common; + using namespace common; const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); @@ -97,7 +97,7 @@ unsigned int GetPrimitiveType( ///////////////////////////////////////////////// CustomMeshShape::CustomMeshShape( - const ignition::common::Mesh &_input, + const common::Mesh &_input, const Eigen::Vector3d &_scale) : dart::dynamics::MeshShape(_scale, nullptr) { @@ -123,7 +123,7 @@ CustomMeshShape::CustomMeshShape( // Fill in submesh contents for (unsigned int i = 0; i < numSubMeshes; ++i) { - const ignition::common::SubMeshPtr &inputSubmesh = + const common::SubMeshPtr &inputSubmesh = _input.SubMeshByIndex(i).lock(); scene->mMeshes[i] = nullptr; @@ -205,11 +205,11 @@ CustomMeshShape::CustomMeshShape( for (unsigned int j = 0; j < numVertices; ++j) { - const ignition::math::Vector3d &v = inputSubmesh->Vertex(j); + const math::Vector3d &v = inputSubmesh->Vertex(j); for (unsigned int k = 0; k < 3; ++k) mesh->mVertices[j][k] = static_cast(v[k]); - const ignition::math::Vector3d &n = inputSubmesh->Normal(j); + const math::Vector3d &n = inputSubmesh->Normal(j); for (unsigned int k = 0; k < 3; ++k) mesh->mNormals[j][k] = static_cast(n[k]); } diff --git a/dartsim/src/CustomMeshShape.hh b/dartsim/src/CustomMeshShape.hh index aae37d45c..da6c5bf0f 100644 --- a/dartsim/src/CustomMeshShape.hh +++ b/dartsim/src/CustomMeshShape.hh @@ -26,12 +26,12 @@ namespace physics { namespace dartsim { /// \brief This class creates a custom derivative of dartsim's MeshShape class -/// which allows an ignition::common::Mesh to be converted into a MeshShape that +/// which allows an common::Mesh to be converted into a MeshShape that /// can be used by dartsim. class CustomMeshShape : public dart::dynamics::MeshShape { public: CustomMeshShape( - const ignition::common::Mesh &_input, + const common::Mesh &_input, const Eigen::Vector3d &_scale); }; diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 58fca7d46..173251814 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -32,23 +32,25 @@ #include "KinematicsFeatures.hh" #include "ShapeFeatures.hh" -struct TestFeatureList : ignition::physics::FeatureList< - ignition::physics::dartsim::EntityManagementFeatureList, - ignition::physics::dartsim::JointFeatureList, - ignition::physics::dartsim::KinematicsFeatureList, - ignition::physics::dartsim::ShapeFeatureList +using namespace ignition; + +struct TestFeatureList : physics::FeatureList< + physics::dartsim::EntityManagementFeatureList, + physics::dartsim::JointFeatureList, + physics::dartsim::KinematicsFeatureList, + physics::dartsim::ShapeFeatureList > { }; TEST(EntityManagement_TEST, ConstructEmptyWorld) { - ignition::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); - ignition::plugin::PluginPtr dartsim = + plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); auto engine = - ignition::physics::RequestEngine3d::From(dartsim); + physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); @@ -97,7 +99,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) prismatic->SetVelocity(0, zVel); prismatic->SetAcceleration(0, zAcc); - const ignition::physics::FrameData3d childData = + const physics::FrameData3d childData = child->FrameDataRelativeToWorld(); const Eigen::Vector3d childPosition = childData.pose.translation(); @@ -120,7 +122,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) childSpherePose.translate(Eigen::Vector3d(0.0, yPos, 0.0)); auto sphere = child->AttachSphereShape("child sphere", 1.0, childSpherePose); - const ignition::physics::FrameData3d sphereData = + const physics::FrameData3d sphereData = sphere->FrameDataRelativeToWorld(); const Eigen::Vector3d spherePosition = sphereData.pose.translation(); @@ -138,7 +140,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_DOUBLE_EQ(0.0, sphereAcceleration.y()); EXPECT_DOUBLE_EQ(zAcc, sphereAcceleration.z()); - const ignition::physics::FrameData3d relativeSphereData = + const physics::FrameData3d relativeSphereData = sphere->FrameDataRelativeTo(*child); const Eigen::Vector3d relativeSpherePosition = relativeSphereData.pose.translation(); @@ -150,7 +152,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) meshLink->AttachFixedJoint(child, "fixed"); const std::string meshFilename = IGNITION_PHYSICS_RESOURCE_DIR "/chassis.dae"; - auto &meshManager = *ignition::common::MeshManager::Instance(); + auto &meshManager = *common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); auto meshShape = meshLink->AttachMeshShape("chassis", *mesh); @@ -167,11 +169,11 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_NEAR(meshShapeSize[1], 0.3831, 1e-4); EXPECT_NEAR(meshShapeSize[2], 0.1956, 1e-4); - const ignition::math::Pose3d pose(0, 0, 0.2, 0, 0, 0); - const ignition::math::Vector3d scale(0.5, 1.0, 0.25); + const math::Pose3d pose(0, 0, 0.2, 0, 0, 0); + const math::Vector3d scale(0.5, 1.0, 0.25); auto meshShapeScaled = meshLink->AttachMeshShape("small_chassis", *mesh, - ignition::math::eigen3::convert(pose), - ignition::math::eigen3::convert(scale)); + math::eigen3::convert(pose), + math::eigen3::convert(scale)); const auto meshShapeScaledSize = meshShapeScaled->GetSize(); // Note: dartsim uses assimp for storing mesh data, and assimp by default uses @@ -187,14 +189,14 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) TEST(EntityManagement_TEST, RemoveEntities) { - ignition::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); - ignition::plugin::PluginPtr dartsim = + plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); auto engine = - ignition::physics::RequestEngine3d::From(dartsim); + physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); diff --git a/dartsim/src/LinkFeatures_TEST.cc b/dartsim/src/LinkFeatures_TEST.cc index c0015daf9..ae826b749 100644 --- a/dartsim/src/LinkFeatures_TEST.cc +++ b/dartsim/src/LinkFeatures_TEST.cc @@ -39,15 +39,17 @@ #include "test/Utils.hh" -struct TestFeatureList : ignition::physics::FeatureList< - ignition::physics::AddLinkExternalForceTorque, - ignition::physics::ForwardStep, - ignition::physics::sdf::ConstructSdfWorld, - ignition::physics::sdf::ConstructSdfModel, - ignition::physics::sdf::ConstructSdfLink, - ignition::physics::GetEntities, - ignition::physics::GetLinkBoundingBox, - ignition::physics::GetModelBoundingBox +using namespace ignition; + +struct TestFeatureList : physics::FeatureList< + physics::AddLinkExternalForceTorque, + physics::ForwardStep, + physics::sdf::ConstructSdfWorld, + physics::sdf::ConstructSdfModel, + physics::sdf::ConstructSdfLink, + physics::GetEntities, + physics::GetLinkBoundingBox, + physics::GetModelBoundingBox > { }; using namespace gz; diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 827a11dcd..7e145ad9e 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -154,7 +154,7 @@ static Eigen::Vector3d ConvertJointAxis( // Error while Resolving xyz. Fallback sdformat 1.6 behavior but treat // xyz_expressed_in = "__model__" as the old use_parent_model_frame - const Eigen::Vector3d axis = ignition::math::eigen3::convert(_sdfAxis->Xyz()); + const Eigen::Vector3d axis = math::eigen3::convert(_sdfAxis->Xyz()); if (_sdfAxis->XyzExpressedIn().empty()) return axis; @@ -323,7 +323,7 @@ Identity SDFFeatures::ConstructSdfWorld( const dart::simulation::WorldPtr &world = this->worlds.at(worldID); - world->setGravity(ignition::math::eigen3::convert(_sdfWorld.Gravity())); + world->setGravity(math::eigen3::convert(_sdfWorld.Gravity())); // TODO(MXG): Add a Physics class to the SDFormat DOM and then parse that // information here. For now, we'll just use dartsim's default physics @@ -414,11 +414,11 @@ Identity SDFFeatures::ConstructSdfLink( dart::dynamics::BodyNode::Properties bodyProperties; bodyProperties.mName = _sdfLink.Name(); - const ignition::math::Inertiald &sdfInertia = _sdfLink.Inertial(); + const math::Inertiald &sdfInertia = _sdfLink.Inertial(); bodyProperties.mInertia.setMass(sdfInertia.MassMatrix().Mass()); // TODO(addisu) Resolve the pose of inertials when frame information is - // availabile for ignition::math::Inertial + // availabile for math::Inertial const Eigen::Matrix3d R_inertial{ math::eigen3::convert(sdfInertia.Pose().Rot())}; @@ -660,7 +660,7 @@ Identity SDFFeatures::ConstructSdfVisual( // intended for the physics? if (_visual.Material()) { - const ignition::math::Color &color = _visual.Material()->Ambient(); + const math::Color &color = _visual.Material()->Ambient(); node->getVisualAspect()->setColor( Eigen::Vector4d(color.R(), color.G(), color.B(), color.A())); } diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 303f6cf6e..1a09dbcc4 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -44,30 +44,32 @@ #include -struct TestFeatureList : ignition::physics::FeatureList< - ignition::physics::GetEntities, - ignition::physics::GetBasicJointState, - ignition::physics::SetBasicJointState, - ignition::physics::dartsim::RetrieveWorld, - ignition::physics::sdf::ConstructSdfJoint, - ignition::physics::sdf::ConstructSdfLink, - ignition::physics::sdf::ConstructSdfModel, - ignition::physics::sdf::ConstructSdfWorld +using namespace ignition; + +struct TestFeatureList : physics::FeatureList< + physics::GetEntities, + physics::GetBasicJointState, + physics::SetBasicJointState, + physics::dartsim::RetrieveWorld, + physics::sdf::ConstructSdfJoint, + physics::sdf::ConstructSdfLink, + physics::sdf::ConstructSdfModel, + physics::sdf::ConstructSdfWorld > { }; -using World = ignition::physics::World3d; -using WorldPtr = ignition::physics::World3dPtr; +using World = physics::World3d; +using WorldPtr = physics::World3dPtr; auto LoadEngine() { - ignition::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); - ignition::plugin::PluginPtr dartsim = + plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); auto engine = - ignition::physics::RequestEngine3d::From(dartsim); + physics::RequestEngine3d::From(dartsim); return engine; } @@ -359,7 +361,7 @@ TEST(SDFFeatures_FrameSemantics, LinkRelativeTo) dartWorld->step(); // Step once and check - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expWorldPose, link2->getWorldTransform(), 1e-3)); } @@ -388,13 +390,13 @@ TEST(SDFFeatures_FrameSemantics, CollisionRelativeTo) Eigen::Isometry3d expPose; expPose = Eigen::Translation3d(0, 0, -1); - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); // Step once and check, the relative pose should still be the same dartWorld->step(); - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); } @@ -423,7 +425,7 @@ TEST(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) Eigen::Isometry3d link1ExpPose; link1ExpPose = Eigen::Translation3d(1, 0, 1); - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( link1ExpPose, link1->getWorldTransform(), 1e-5)); // Expect the world pose of L2 to be the same as the world pose of F2, which @@ -431,15 +433,15 @@ TEST(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) Eigen::Isometry3d link2ExpPose; link2ExpPose = Eigen::Translation3d(1, 0, 0); - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( link2ExpPose, link2->getWorldTransform(), 1e-5)); // Step once and check dartWorld->step(); - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( link1ExpPose, link1->getWorldTransform(), 1e-5)); - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( link2ExpPose, link2->getWorldTransform(), 1e-5)); } @@ -468,13 +470,13 @@ TEST(SDFFeatures_FrameSemantics, ExplicitFramesWithCollision) Eigen::Isometry3d expPose; expPose = Eigen::Translation3d(0, 0, 1); - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); // Step once and check dartWorld->step(); - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); } @@ -503,13 +505,13 @@ TEST(SDFFeatures_FrameSemantics, ExplicitWorldFrames) // Since we can't get the skeleton's world transform, we use the world // transform of L1 which is at the origin of the model frame. - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, link1->getWorldTransform(), 1e-5)); // Step once and check dartWorld->step(); - EXPECT_TRUE(ignition::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, link1->getWorldTransform(), 1e-5)); } diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 6d33a4de2..537e1d8e8 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -246,7 +246,7 @@ LinearVector3d ShapeFeatures::GetMeshShapeScale( Identity ShapeFeatures::AttachMeshShape( const Identity &_linkID, const std::string &_name, - const ignition::common::Mesh &_mesh, + const common::Mesh &_mesh, const Pose3d &_pose, const LinearVector3d &_scale) { diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index 4809a4618..2fd95042b 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -135,7 +135,7 @@ class ShapeFeatures : public: Identity AttachMeshShape( const Identity &_linkID, const std::string &_name, - const ignition::common::Mesh &_mesh, + const common::Mesh &_mesh, const Pose3d &_pose, const LinearVector3d &_scale) override; diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc index 6c3b4e6fe..3cd6eb297 100644 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ b/dartsim/src/SimulationFeatures_TEST.cc @@ -42,28 +42,30 @@ #include #include -struct TestFeatureList : ignition::physics::FeatureList< - ignition::physics::LinkFrameSemantics, - ignition::physics::ForwardStep, - ignition::physics::GetContactsFromLastStepFeature, - ignition::physics::GetEntities, - ignition::physics::GetShapeBoundingBox, - ignition::physics::CollisionFilterMaskFeature, +using namespace ignition; + +struct TestFeatureList : physics::FeatureList< + physics::LinkFrameSemantics, + physics::ForwardStep, + physics::GetContactsFromLastStepFeature, + physics::GetEntities, + physics::GetShapeBoundingBox, + physics::CollisionFilterMaskFeature, #ifdef DART_HAS_CONTACT_SURFACE - ignition::physics::SetContactPropertiesCallbackFeature, + physics::SetContactPropertiesCallbackFeature, #endif - ignition::physics::sdf::ConstructSdfWorld + physics::sdf::ConstructSdfWorld > { }; -using TestWorldPtr = ignition::physics::World3dPtr; -using TestShapePtr = ignition::physics::Shape3dPtr; -using TestWorld = ignition::physics::World3d; +using TestWorldPtr = physics::World3dPtr; +using TestShapePtr = physics::Shape3dPtr; +using TestWorld = physics::World3d; using TestContactPoint = TestWorld::ContactPoint; using TestExtraContactData = TestWorld::ExtraContactData; using TestContact = TestWorld::Contact; #ifdef DART_HAS_CONTACT_SURFACE using ContactSurfaceParams = - ignition::physics::SetContactPropertiesCallbackFeature:: + physics::SetContactPropertiesCallbackFeature:: ContactSurfaceParams; #endif @@ -71,23 +73,23 @@ std::unordered_set LoadWorlds( const std::string &_library, const std::string &_world) { - ignition::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(_library); const std::set pluginNames = - ignition::physics::FindFeatures3d::From(loader); + physics::FindFeatures3d::From(loader); EXPECT_LT(0u, pluginNames.size()); std::unordered_set worlds; for (const std::string &name : pluginNames) { - ignition::plugin::PluginPtr plugin = loader.Instantiate(name); + plugin::PluginPtr plugin = loader.Instantiate(name); std::cout << " -- Plugin name: " << name << std::endl; auto engine = - ignition::physics::RequestEngine3d::From(plugin); + physics::RequestEngine3d::From(plugin); EXPECT_NE(nullptr, engine); sdf::Root root; @@ -104,9 +106,9 @@ std::unordered_set LoadWorlds( void StepWorld(const TestWorldPtr &_world, const std::size_t _num_steps = 1) { - ignition::physics::ForwardStep::Input input; - ignition::physics::ForwardStep::State state; - ignition::physics::ForwardStep::Output output; + physics::ForwardStep::Input input; + physics::ForwardStep::State state; + physics::ForwardStep::Output output; for (size_t i = 0; i < _num_steps; ++i) { @@ -161,14 +163,14 @@ TEST_P(SimulationFeatures_TEST, ShapeBoundingBox) auto groundAABB = groundCollision->GetAxisAlignedBoundingBox(*groundCollision); - EXPECT_EQ(ignition::math::Vector3d(-1, -1, -1), - ignition::math::eigen3::convert(sphereAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), - ignition::math::eigen3::convert(sphereAABB).Max()); - EXPECT_EQ(ignition::math::Vector3d(-50, -50, -0.5), - ignition::math::eigen3::convert(groundAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(50, 50, 0.5), - ignition::math::eigen3::convert(groundAABB).Max()); + EXPECT_EQ(math::Vector3d(-1, -1, -1), + math::eigen3::convert(sphereAABB).Min()); + EXPECT_EQ(math::Vector3d(1, 1, 1), + math::eigen3::convert(sphereAABB).Max()); + EXPECT_EQ(math::Vector3d(-50, -50, -0.5), + math::eigen3::convert(groundAABB).Min()); + EXPECT_EQ(math::Vector3d(50, 50, 0.5), + math::eigen3::convert(groundAABB).Max()); // Test the bounding boxes in the world frames sphereAABB = sphereCollision->GetAxisAlignedBoundingBox(); @@ -179,14 +181,14 @@ TEST_P(SimulationFeatures_TEST, ShapeBoundingBox) // a 45-degree rotation, the dimensions that are orthogonal to the axis of // rotation will dilate from 1.0 to sqrt(2). const double d = std::sqrt(2); - EXPECT_EQ(ignition::math::Vector3d(-d, -1, 2.0 - d), - ignition::math::eigen3::convert(sphereAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(d, 1, 2 + d), - ignition::math::eigen3::convert(sphereAABB).Max()); - EXPECT_EQ(ignition::math::Vector3d(-50*d, -50*d, -1), - ignition::math::eigen3::convert(groundAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(50*d, 50*d, 0), - ignition::math::eigen3::convert(groundAABB).Max()); + EXPECT_EQ(math::Vector3d(-d, -1, 2.0 - d), + math::eigen3::convert(sphereAABB).Min()); + EXPECT_EQ(math::Vector3d(d, 1, 2 + d), + math::eigen3::convert(sphereAABB).Max()); + EXPECT_EQ(math::Vector3d(-50*d, -50*d, -1), + math::eigen3::convert(groundAABB).Min()); + EXPECT_EQ(math::Vector3d(50*d, 50*d, 0), + math::eigen3::convert(groundAABB).Max()); } } @@ -302,7 +304,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) expectedContactPos = expectations.at(testCollision); - EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); // Check if the engine populated the extra contact data struct @@ -361,10 +363,10 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_NEAR(_surfaceParams.secondarySlipCompliance.value(), 0.0, 1e-6); EXPECT_NEAR(_surfaceParams.restitutionCoeff.value(), 0.0, 1e-6); - EXPECT_TRUE(ignition::physics::test::Equal(Eigen::Vector3d(0, 0, 1), + EXPECT_TRUE(physics::test::Equal(Eigen::Vector3d(0, 0, 1), _surfaceParams.firstFrictionalDirection.value(), 1e-6)); - EXPECT_TRUE(ignition::physics::test::Equal(Eigen::Vector3d(0, 0, 0), + EXPECT_TRUE(physics::test::Equal(Eigen::Vector3d(0, 0, 0), _surfaceParams.contactSurfaceMotionVelocity.value(), 1e-6)); }; world->AddContactPropertiesCallback("test", contactCallback); @@ -456,7 +458,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) } INSTANTIATE_TEST_CASE_P(PhysicsPlugins, SimulationFeatures_TEST, - ::testing::ValuesIn(ignition::physics::test::g_PhysicsPluginLibraries),); // NOLINT + ::testing::ValuesIn(physics::test::g_PhysicsPluginLibraries),); // NOLINT int main(int argc, char *argv[]) { diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh index 09c92ec1d..c83d324ac 100644 --- a/tpe/lib/src/Shape.hh +++ b/tpe/lib/src/Shape.hh @@ -182,7 +182,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE MeshShape : public Shape /// \brief Set mesh /// \param[in] _mesh Mesh object - public: void SetMesh(const ignition::common::Mesh &_mesh); + public: void SetMesh(const common::Mesh &_mesh); /// \brief Get mesh scale /// \return Mesh scale diff --git a/tpe/plugin/src/EntityManagement_TEST.cc b/tpe/plugin/src/EntityManagement_TEST.cc index e0250c121..92e9a400a 100644 --- a/tpe/plugin/src/EntityManagement_TEST.cc +++ b/tpe/plugin/src/EntityManagement_TEST.cc @@ -23,21 +23,23 @@ #include "EntityManagementFeatures.hh" -struct TestFeatureList : ignition::physics::FeatureList< - ignition::physics::tpeplugin::EntityManagementFeatureList +using namespace ignition; + +struct TestFeatureList : physics::FeatureList< + physics::tpeplugin::EntityManagementFeatureList > { }; TEST(EntityManagement_TEST, ConstructEmptyWorld) { - ignition::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(tpe_plugin_LIB); - ignition::plugin::PluginPtr tpe_plugin = + plugin::PluginPtr tpe_plugin = loader.Instantiate("ignition::physics::tpeplugin::Plugin"); // GetEntities_TEST auto engine = - ignition::physics::RequestEngine3d::From(tpe_plugin); + physics::RequestEngine3d::From(tpe_plugin); ASSERT_NE(nullptr, engine); EXPECT_EQ("tpe", engine->GetName()); EXPECT_EQ(0u, engine->GetIndex()); @@ -83,14 +85,14 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) TEST(EntityManagement_TEST, RemoveEntities) { - ignition::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(tpe_plugin_LIB); - ignition::plugin::PluginPtr tpe_plugin = + plugin::PluginPtr tpe_plugin = loader.Instantiate("ignition::physics::tpeplugin::Plugin"); auto engine = - ignition::physics::RequestEngine3d::From(tpe_plugin); + physics::RequestEngine3d::From(tpe_plugin); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); diff --git a/tpe/plugin/src/SDFFeatures_TEST.cc b/tpe/plugin/src/SDFFeatures_TEST.cc index e6f467ba9..10c3a775d 100644 --- a/tpe/plugin/src/SDFFeatures_TEST.cc +++ b/tpe/plugin/src/SDFFeatures_TEST.cc @@ -41,28 +41,30 @@ #include "lib/src/World.hh" #include "World.hh" -struct TestFeatureList : ignition::physics::FeatureList< - ignition::physics::tpeplugin::RetrieveWorld, - ignition::physics::GetModelFromWorld, - ignition::physics::sdf::ConstructSdfLink, - ignition::physics::sdf::ConstructSdfModel, - ignition::physics::sdf::ConstructSdfNestedModel, - ignition::physics::sdf::ConstructSdfWorld +using namespace ignition; + +struct TestFeatureList : physics::FeatureList< + physics::tpeplugin::RetrieveWorld, + physics::GetModelFromWorld, + physics::sdf::ConstructSdfLink, + physics::sdf::ConstructSdfModel, + physics::sdf::ConstructSdfNestedModel, + physics::sdf::ConstructSdfWorld > { }; -using World = ignition::physics::World3d; -using WorldPtr = ignition::physics::World3dPtr; +using World = physics::World3d; +using WorldPtr = physics::World3dPtr; auto LoadEngine() { - ignition::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(tpe_plugin_LIB); - ignition::plugin::PluginPtr tpe_plugin = + plugin::PluginPtr tpe_plugin = loader.Instantiate("ignition::physics::tpeplugin::Plugin"); auto engine = - ignition::physics::RequestEngine3d::From(tpe_plugin); + physics::RequestEngine3d::From(tpe_plugin); return engine; } @@ -96,264 +98,264 @@ TEST(SDFFeatures_TEST, CheckTpeData) // check model 01 { - ignition::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("ground_plane"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("ground_plane", model.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, model.GetPose()); - EXPECT_EQ(ignition::math::Pose3d::Zero, model.GetWorldPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetWorldPose()); EXPECT_EQ(1u, model.GetChildCount()); - ignition::physics::tpelib::Entity &link = model.GetChildByName("link"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link", link.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetWorldPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetWorldPose()); EXPECT_EQ(1u, link.GetChildCount()); - ignition::physics::tpelib::Entity &collision = + physics::tpelib::Entity &collision = link.GetChildByName("collision"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision.GetId()); EXPECT_EQ("collision", collision.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, collision.GetPose()); - EXPECT_EQ(ignition::math::Pose3d::Zero, collision.GetWorldPose()); + EXPECT_EQ(math::Pose3d::Zero, collision.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, collision.GetWorldPose()); } // check model 02 { - ignition::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("double_pendulum_with_base"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("double_pendulum_with_base", model.GetName()); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetPose()); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0, 0, 0, 0), model.GetPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); EXPECT_EQ(3u, model.GetChildCount()); - ignition::physics::tpelib::Entity &link = model.GetChildByName("base"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("base"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("base", link.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose()); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); EXPECT_EQ(2u, link.GetChildCount()); - ignition::physics::tpelib::Entity &collision = + physics::tpelib::Entity &collision = link.GetChildByName("col_plate_on_ground"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision.GetId()); EXPECT_EQ("col_plate_on_ground", collision.GetName()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.01, 0, 0, 0), collision.GetPose()); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 0.01, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0.01, 0, 0, 0), collision.GetPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0.01, 0, 0, 0), collision.GetWorldPose()); - ignition::physics::tpelib::Entity &collision02 = + physics::tpelib::Entity &collision02 = link.GetChildByName("col_pole"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision.GetId()); EXPECT_EQ("col_pole", collision02.GetName()); - EXPECT_EQ(ignition::math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), collision02.GetPose()); - EXPECT_EQ(ignition::math::Pose3d(0.725, 0, 1.1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.725, 0, 1.1, 0, 0, 0), collision02.GetWorldPose()); - ignition::physics::tpelib::Entity &link02 = + physics::tpelib::Entity &link02 = model.GetChildByName("upper_link"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("upper_link", link02.GetName()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 2.1, -1.5708, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 2.1, -1.5708, 0, 0), link02.GetPose()); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 2.1, -1.5708, 0, 0), + EXPECT_EQ(math::Pose3d(1, 0, 2.1, -1.5708, 0, 0), link02.GetWorldPose()); EXPECT_EQ(3u, link02.GetChildCount()); - ignition::physics::tpelib::Entity &collision03 = + physics::tpelib::Entity &collision03 = link02.GetChildByName("col_upper_joint"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision03.GetId()); EXPECT_EQ("col_upper_joint", collision03.GetName()); - EXPECT_EQ(ignition::math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0), + EXPECT_EQ(math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0), collision03.GetPose()); EXPECT_EQ( - ignition::math::Pose3d(0.95, 0, 2.1, -1.5708, -4e-06, -1.5708), + math::Pose3d(0.95, 0, 2.1, -1.5708, -4e-06, -1.5708), collision03.GetWorldPose()); - ignition::physics::tpelib::Entity &collision04 = + physics::tpelib::Entity &collision04 = link02.GetChildByName("col_lower_joint"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision04.GetId()); EXPECT_EQ("col_lower_joint", collision04.GetName()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 1.0, 0, 1.5708, 0), + EXPECT_EQ(math::Pose3d(0, 0, 1.0, 0, 1.5708, 0), collision04.GetPose()); EXPECT_EQ( - ignition::math::Pose3d(1, 1, 2.1, -1.5708, -4e-06, -1.5708), + math::Pose3d(1, 1, 2.1, -1.5708, -4e-06, -1.5708), collision04.GetWorldPose()); - ignition::physics::tpelib::Entity &collision05 = + physics::tpelib::Entity &collision05 = link02.GetChildByName("col_cylinder"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision05.GetId()); EXPECT_EQ("col_cylinder", collision05.GetName()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0.5, 0, 0, 0), collision05.GetPose()); - EXPECT_EQ(ignition::math::Pose3d(1, 0.5, 2.1, -1.5708, 0, 0), + EXPECT_EQ(math::Pose3d(1, 0.5, 2.1, -1.5708, 0, 0), collision05.GetWorldPose()); - ignition::physics::tpelib::Entity &link03 = + physics::tpelib::Entity &link03 = model.GetChildByName("lower_link"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link03.GetId()); EXPECT_EQ("lower_link", link03.GetName()); - EXPECT_EQ(ignition::math::Pose3d(0.25, 1.0, 2.1, -2, 0, 0), + EXPECT_EQ(math::Pose3d(0.25, 1.0, 2.1, -2, 0, 0), link03.GetPose()); - EXPECT_EQ(ignition::math::Pose3d(1.25, 1.0, 2.1, -2, 0, 0), + EXPECT_EQ(math::Pose3d(1.25, 1.0, 2.1, -2, 0, 0), link03.GetWorldPose()); EXPECT_EQ(2u, link03.GetChildCount()); - ignition::physics::tpelib::Entity &collision06 = + physics::tpelib::Entity &collision06 = link03.GetChildByName("col_lower_joint"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision06.GetId()); EXPECT_EQ("col_lower_joint", collision06.GetName()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 1.5708, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 1.5708, 0), collision06.GetPose()); EXPECT_EQ( - ignition::math::Pose3d(1.25, 1, 2.1, -1.57079, -0.429204, -1.5708), + math::Pose3d(1.25, 1, 2.1, -1.57079, -0.429204, -1.5708), collision06.GetWorldPose()); - ignition::physics::tpelib::Entity &collision07 = + physics::tpelib::Entity &collision07 = link03.GetChildByName("col_cylinder"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision07.GetId()); EXPECT_EQ("col_cylinder", collision07.GetName()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0.5, 0, 0, 0), collision07.GetPose()); - EXPECT_EQ(ignition::math::Pose3d(1.25, 1.45465, 1.89193, -2, 0, 0), + EXPECT_EQ(math::Pose3d(1.25, 1.45465, 1.89193, -2, 0, 0), collision07.GetWorldPose()); } // check model 03 { - ignition::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("free_body"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("free_body", model.GetName()); - EXPECT_EQ(ignition::math::Pose3d(0, 10, 10, 0, 0, 0), model.GetPose()); + EXPECT_EQ(math::Pose3d(0, 10, 10, 0, 0, 0), model.GetPose()); EXPECT_EQ(1u, model.GetChildCount()); - ignition::physics::tpelib::Entity &link = model.GetChildByName("link"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link", link.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(0u, link.GetChildCount()); } // check model 04 { - ignition::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("joint_limit_test"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("joint_limit_test", model.GetName()); - EXPECT_EQ(ignition::math::Pose3d(10, 0, 2, 0, 0, 0), model.GetPose()); + EXPECT_EQ(math::Pose3d(10, 0, 2, 0, 0, 0), model.GetPose()); EXPECT_EQ(2u, model.GetChildCount()); - ignition::physics::tpelib::Entity &link = model.GetChildByName("base"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("base"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("base", link.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(0u, link.GetChildCount()); - ignition::physics::tpelib::Entity &link02 = model.GetChildByName("bar"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link02 = model.GetChildByName("bar"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("bar", link02.GetName()); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), link02.GetPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0, 0, 0, 0), link02.GetPose()); EXPECT_EQ(0u, link02.GetChildCount()); } // check model 05 { - ignition::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("screw_joint_test"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("screw_joint_test", model.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, model.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); EXPECT_EQ(2u, model.GetChildCount()); - ignition::physics::tpelib::Entity &link = model.GetChildByName("link0"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link0"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link0", link.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(0u, link.GetChildCount()); - ignition::physics::tpelib::Entity &link02 = model.GetChildByName("link1"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link02 = model.GetChildByName("link1"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("link1", link02.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link02.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link02.GetPose()); EXPECT_EQ(0u, link02.GetChildCount()); } // check model 06 { - ignition::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("unsupported_joint_test"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("unsupported_joint_test", model.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, model.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); EXPECT_EQ(6u, model.GetChildCount()); - ignition::physics::tpelib::Entity &link = model.GetChildByName("link0"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link0"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link0", link.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(0u, link.GetChildCount()); - ignition::physics::tpelib::Entity &link02 = model.GetChildByName("link1"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link02 = model.GetChildByName("link1"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("link1", link02.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link02.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link02.GetPose()); EXPECT_EQ(0u, link02.GetChildCount()); - ignition::physics::tpelib::Entity &link03 = model.GetChildByName("link2"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link03 = model.GetChildByName("link2"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link03.GetId()); EXPECT_EQ("link2", link03.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link03.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link03.GetPose()); EXPECT_EQ(0u, link03.GetChildCount()); - ignition::physics::tpelib::Entity &link04 = model.GetChildByName("link3"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link04 = model.GetChildByName("link3"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link04.GetId()); EXPECT_EQ("link3", link04.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link04.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link04.GetPose()); EXPECT_EQ(0u, link04.GetChildCount()); - ignition::physics::tpelib::Entity &link05 = model.GetChildByName("link4"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link05 = model.GetChildByName("link4"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link05.GetId()); EXPECT_EQ("link4", link05.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link05.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link05.GetPose()); EXPECT_EQ(0u, link05.GetChildCount()); - ignition::physics::tpelib::Entity &link06 = model.GetChildByName("link5"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link06 = model.GetChildByName("link5"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link06.GetId()); EXPECT_EQ("link5", link06.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link06.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link06.GetPose()); EXPECT_EQ(0u, link06.GetChildCount()); } } @@ -368,57 +370,57 @@ TEST(SDFFeatures_TEST, NestedModel) ASSERT_EQ(1u, tpeWorld->GetChildCount()); // check top level model - ignition::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("model"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("model", model.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, model.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); EXPECT_EQ(2u, model.GetChildCount()); - ignition::physics::tpelib::Entity &link = model.GetChildByName("link"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link", link.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(1u, link.GetChildCount()); - ignition::physics::tpelib::Entity &collision = + physics::tpelib::Entity &collision = link.GetChildByName("collision"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision.GetId()); EXPECT_EQ("collision", collision.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, collision.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, collision.GetPose()); // check nested model - ignition::physics::tpelib::Entity &nestedModel = + physics::tpelib::Entity &nestedModel = model.GetChildByName("nested_model"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedModel.GetId()); EXPECT_EQ("nested_model", nestedModel.GetName()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 2, 0, 0, 0), nestedModel.GetPose()); + EXPECT_EQ(math::Pose3d(1, 2, 2, 0, 0, 0), nestedModel.GetPose()); EXPECT_EQ(1u, nestedModel.GetChildCount()); - ignition::physics::tpelib::Entity &nestedLink = + physics::tpelib::Entity &nestedLink = nestedModel.GetChildByName("nested_link"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedLink.GetId()); EXPECT_EQ("nested_link", nestedLink.GetName()); - EXPECT_EQ(ignition::math::Pose3d(3, 1, 1, 0, 0, 1.5707), + EXPECT_EQ(math::Pose3d(3, 1, 1, 0, 0, 1.5707), nestedLink.GetPose()); EXPECT_EQ(1u, nestedLink.GetChildCount()); - ignition::physics::tpelib::Entity &nestedCollision = + physics::tpelib::Entity &nestedCollision = nestedLink.GetChildByName("nested_collision"); - ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedCollision.GetId()); EXPECT_EQ("nested_collision", nestedCollision.GetName()); - EXPECT_EQ(ignition::math::Pose3d::Zero, nestedCollision.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, nestedCollision.GetPose()); // canonical link - ignition::physics::tpelib::Model *m = - static_cast(&model); - ignition::physics::tpelib::Entity canLink = m->GetCanonicalLink(); + physics::tpelib::Model *m = + static_cast(&model); + physics::tpelib::Entity canLink = m->GetCanonicalLink(); EXPECT_EQ(link.GetId(), canLink.GetId()); } diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index f4e713fd7..faa194570 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -41,42 +41,44 @@ #include "ShapeFeatures.hh" #include "SimulationFeatures.hh" -struct TestFeatureList : ignition::physics::FeatureList< - ignition::physics::tpeplugin::SimulationFeatureList, - ignition::physics::tpeplugin::ShapeFeatureList, - ignition::physics::tpeplugin::EntityManagementFeatureList, - ignition::physics::tpeplugin::FreeGroupFeatureList, - ignition::physics::GetContactsFromLastStepFeature, - ignition::physics::LinkFrameSemantics, - ignition::physics::GetModelBoundingBox, - ignition::physics::sdf::ConstructSdfWorld +using namespace ignition; + +struct TestFeatureList : physics::FeatureList< + physics::tpeplugin::SimulationFeatureList, + physics::tpeplugin::ShapeFeatureList, + physics::tpeplugin::EntityManagementFeatureList, + physics::tpeplugin::FreeGroupFeatureList, + physics::GetContactsFromLastStepFeature, + physics::LinkFrameSemantics, + physics::GetModelBoundingBox, + physics::sdf::ConstructSdfWorld > { }; -using TestWorldPtr = ignition::physics::World3dPtr; -using TestShapePtr = ignition::physics::Shape3dPtr; -using TestContactPoint = ignition::physics::World3d::ContactPoint; +using TestWorldPtr = physics::World3dPtr; +using TestShapePtr = physics::Shape3dPtr; +using TestContactPoint = physics::World3d::ContactPoint; std::unordered_set LoadWorlds( const std::string &_library, const std::string &_world) { - ignition::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(_library); const std::set pluginNames = - ignition::physics::FindFeatures3d::From(loader); + physics::FindFeatures3d::From(loader); EXPECT_EQ(1u, pluginNames.size()); std::unordered_set worlds; for (const std::string &name : pluginNames) { - ignition::plugin::PluginPtr plugin = loader.Instantiate(name); + plugin::PluginPtr plugin = loader.Instantiate(name); igndbg << " -- Plugin name: " << name << std::endl; auto engine = - ignition::physics::RequestEngine3d::From(plugin); + physics::RequestEngine3d::From(plugin); EXPECT_NE(nullptr, engine); sdf::Root root; @@ -92,9 +94,9 @@ std::unordered_set LoadWorlds( void StepWorld(const TestWorldPtr &_world, const std::size_t _num_steps = 1) { - ignition::physics::ForwardStep::Input input; - ignition::physics::ForwardStep::State state; - ignition::physics::ForwardStep::Output output; + physics::ForwardStep::Input input; + physics::ForwardStep::State state; + physics::ForwardStep::Output output; for (size_t i = 0; i < _num_steps; ++i) { @@ -126,8 +128,8 @@ TEST_P(SimulationFeatures_TEST, StepWorld) auto link = model->GetLink(0); ASSERT_NE(nullptr, link); auto frameData = link->FrameDataRelativeToWorld(); - EXPECT_EQ(ignition::math::Pose3d(0, 1.5, 0.5, 0, 0, 0), - ignition::math::eigen3::convert(frameData.pose)); + EXPECT_EQ(math::Pose3d(0, 1.5, 0.5, 0, 0, 0), + math::eigen3::convert(frameData.pose)); } } @@ -158,13 +160,13 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) auto boxLink = box->GetLink(0); auto boxCollision = boxLink->GetShape(0); auto boxShape = boxCollision->CastToBoxShape(); - EXPECT_EQ(ignition::math::Vector3d(100, 100, 1), - ignition::math::eigen3::convert(boxShape->GetSize())); + EXPECT_EQ(math::Vector3d(100, 100, 1), + math::eigen3::convert(boxShape->GetSize())); auto box2 = boxLink->AttachBoxShape( "box2", - ignition::math::eigen3::convert( - ignition::math::Vector3d(1.2, 1.2, 1.2)), + math::eigen3::convert( + math::Vector3d(1.2, 1.2, 1.2)), Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, boxLink->GetShapeCount()); EXPECT_EQ(box2, boxLink->GetShape(1)); @@ -189,35 +191,35 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) auto cylinderAABB = cylinderCollision->GetAxisAlignedBoundingBox(*cylinderCollision); - EXPECT_EQ(ignition::math::Vector3d(-1, -1, -1), - ignition::math::eigen3::convert(sphereAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), - ignition::math::eigen3::convert(sphereAABB).Max()); - EXPECT_EQ(ignition::math::Vector3d(-50, -50, -0.5), - ignition::math::eigen3::convert(boxAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(50, 50, 0.5), - ignition::math::eigen3::convert(boxAABB).Max()); - EXPECT_EQ(ignition::math::Vector3d(-0.5, -0.5, -0.55), - ignition::math::eigen3::convert(cylinderAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.5, 0.55), - ignition::math::eigen3::convert(cylinderAABB).Max()); + EXPECT_EQ(math::Vector3d(-1, -1, -1), + math::eigen3::convert(sphereAABB).Min()); + EXPECT_EQ(math::Vector3d(1, 1, 1), + math::eigen3::convert(sphereAABB).Max()); + EXPECT_EQ(math::Vector3d(-50, -50, -0.5), + math::eigen3::convert(boxAABB).Min()); + EXPECT_EQ(math::Vector3d(50, 50, 0.5), + math::eigen3::convert(boxAABB).Max()); + EXPECT_EQ(math::Vector3d(-0.5, -0.5, -0.55), + math::eigen3::convert(cylinderAABB).Min()); + EXPECT_EQ(math::Vector3d(0.5, 0.5, 0.55), + math::eigen3::convert(cylinderAABB).Max()); // check model AABB. By default, the AABBs are in world frame auto sphereModelAABB = sphere->GetAxisAlignedBoundingBox(); auto boxModelAABB = box->GetAxisAlignedBoundingBox(); auto cylinderModelAABB = cylinder->GetAxisAlignedBoundingBox(); - EXPECT_EQ(ignition::math::Vector3d(-1, 0.5, -0.5), - ignition::math::eigen3::convert(sphereModelAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(1, 2.5, 1.5), - ignition::math::eigen3::convert(sphereModelAABB).Max()); - EXPECT_EQ(ignition::math::Vector3d(-50, -50, -0.1), - ignition::math::eigen3::convert(boxModelAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(50, 50, 1.1), - ignition::math::eigen3::convert(boxModelAABB).Max()); - EXPECT_EQ(ignition::math::Vector3d(-3, -4.5, -1.5), - ignition::math::eigen3::convert(cylinderModelAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(3, 1.5, 2.5), - ignition::math::eigen3::convert(cylinderModelAABB).Max()); + EXPECT_EQ(math::Vector3d(-1, 0.5, -0.5), + math::eigen3::convert(sphereModelAABB).Min()); + EXPECT_EQ(math::Vector3d(1, 2.5, 1.5), + math::eigen3::convert(sphereModelAABB).Max()); + EXPECT_EQ(math::Vector3d(-50, -50, -0.1), + math::eigen3::convert(boxModelAABB).Min()); + EXPECT_EQ(math::Vector3d(50, 50, 1.1), + math::eigen3::convert(boxModelAABB).Max()); + EXPECT_EQ(math::Vector3d(-3, -4.5, -1.5), + math::eigen3::convert(cylinderModelAABB).Min()); + EXPECT_EQ(math::Vector3d(3, 1.5, 2.5), + math::eigen3::convert(cylinderModelAABB).Max()); } } @@ -242,16 +244,16 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) ASSERT_NE(nullptr, freeGroupLink); freeGroup->SetWorldPose( - ignition::math::eigen3::convert( - ignition::math::Pose3d(0, 0, 2, 0, 0, 0))); + math::eigen3::convert( + math::Pose3d(0, 0, 2, 0, 0, 0))); freeGroup->SetWorldLinearVelocity( - ignition::math::eigen3::convert(ignition::math::Vector3d(0.5, 0, 0.1))); + math::eigen3::convert(math::Vector3d(0.5, 0, 0.1))); freeGroup->SetWorldAngularVelocity( - ignition::math::eigen3::convert(ignition::math::Vector3d(0.1, 0.2, 0))); + math::eigen3::convert(math::Vector3d(0.1, 0.2, 0))); auto frameData = model->GetLink(0)->FrameDataRelativeToWorld(); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 2, 0, 0, 0), - ignition::math::eigen3::convert(frameData.pose)); + EXPECT_EQ(math::Pose3d(0, 0, 2, 0, 0, 0), + math::eigen3::convert(frameData.pose)); } } @@ -341,7 +343,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) { contactBoxSphere++; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, 1.5, 0.5); - EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } else if ((m1->GetName() == "box" && m2->GetName() == "cylinder") || @@ -349,7 +351,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) { contactBoxCylinder++; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5); - EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } else @@ -362,8 +364,8 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_EQ(1u, contactBoxCylinder); // move sphere away - sphereFreeGroup->SetWorldPose(ignition::math::eigen3::convert( - ignition::math::Pose3d(0, 100, 0.5, 0, 0, 0))); + sphereFreeGroup->SetWorldPose(math::eigen3::convert( + math::Pose3d(0, 100, 0.5, 0, 0, 0))); // step and get contacts StepWorld(world, 1); @@ -391,14 +393,14 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) FAIL() << "There should only be contacts between box and cylinder"; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5); - EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } EXPECT_EQ(1u, contactBoxCylinder); // move cylinder away - cylinderFreeGroup->SetWorldPose(ignition::math::eigen3::convert( - ignition::math::Pose3d(0, -100, 0.5, 0, 0, 0))); + cylinderFreeGroup->SetWorldPose(math::eigen3::convert( + math::Pose3d(0, -100, 0.5, 0, 0, 0))); // step and get contacts StepWorld(world, 1); @@ -410,7 +412,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) } INSTANTIATE_TEST_CASE_P(PhysicsPlugins, SimulationFeatures_TEST, - ::testing::ValuesIn(ignition::physics::test::g_PhysicsPluginLibraries),); // NOLINT + ::testing::ValuesIn(physics::test::g_PhysicsPluginLibraries),); // NOLINT int main(int argc, char *argv[]) {