Skip to content

Commit

Permalink
Reduce more use of ignition::
Browse files Browse the repository at this point in the history
Signed-off-by: Nate Koenig <nate@openrobotics.org>
  • Loading branch information
Nate Koenig committed Nov 29, 2022
1 parent 8a24b5d commit 43b7957
Show file tree
Hide file tree
Showing 14 changed files with 357 additions and 341 deletions.
52 changes: 27 additions & 25 deletions dartsim/src/Collisions_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -35,43 +35,45 @@

#include <gz/common/MeshManager.hh>

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<TestFeatureList>;
using TestEnginePtr = ignition::physics::Engine3dPtr<TestFeatureList>;
using TestWorldPtr = physics::World3dPtr<TestFeatureList>;
using TestEnginePtr = physics::Engine3dPtr<TestFeatureList>;

using WorldConstructor = std::function<TestWorldPtr(const TestEnginePtr&)>;

std::unordered_set<TestWorldPtr> LoadWorlds(
const std::string &_library,
const WorldConstructor &_constructor)
{
ignition::plugin::Loader loader;
plugin::Loader loader;
loader.LoadLib(_library);

const std::set<std::string> pluginNames =
ignition::physics::FindFeatures3d<TestFeatureList>::From(loader);
physics::FindFeatures3d<TestFeatureList>::From(loader);

std::unordered_set<TestWorldPtr> 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<TestFeatureList>::From(plugin);
physics::RequestEngine3d<TestFeatureList>::From(plugin);
EXPECT_NE(nullptr, engine);

worlds.insert(_constructor(engine));
Expand All @@ -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<TestFeatureList> &_engine,
const ignition::common::Mesh &_mesh)
const physics::Engine3dPtr<TestFeatureList> &_engine,
const common::Mesh &_mesh)
{
auto world = _engine->ConstructEmptyWorld("world");

Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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);
Expand Down
16 changes: 8 additions & 8 deletions dartsim/src/CustomMeshShape.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down Expand Up @@ -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();

Expand All @@ -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)
{
Expand All @@ -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;
Expand Down Expand Up @@ -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<ai_real>(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<ai_real>(n[k]);
}
Expand Down
4 changes: 2 additions & 2 deletions dartsim/src/CustomMeshShape.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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);
};

Expand Down
40 changes: 21 additions & 19 deletions dartsim/src/EntityManagement_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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<TestFeatureList>::From(dartsim);
physics::RequestEngine3d<TestFeatureList>::From(dartsim);
ASSERT_NE(nullptr, engine);

auto world = engine->ConstructEmptyWorld("empty world");
Expand Down Expand Up @@ -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();
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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);
Expand All @@ -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
Expand All @@ -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<TestFeatureList>::From(dartsim);
physics::RequestEngine3d<TestFeatureList>::From(dartsim);
ASSERT_NE(nullptr, engine);

auto world = engine->ConstructEmptyWorld("empty world");
Expand Down
20 changes: 11 additions & 9 deletions dartsim/src/LinkFeatures_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
10 changes: 5 additions & 5 deletions dartsim/src/SDFFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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())};

Expand Down Expand Up @@ -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()));
}
Expand Down
Loading

0 comments on commit 43b7957

Please sign in to comment.