Set collision detector and solver for DART (#225)

Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Ashton Larkin <[email protected]>

CMakeLists.txt

@@ -68,6 +68,7 @@ ign_find_package(sdformat11
 # Find dartsim for the dartsim plugin wrapper
 ign_find_package(DART
   COMPONENTS
+    collision-bullet
     collision-ode
     utils
     utils-urdf

dartsim/src/WorldFeatures.cc

@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include <memory>
+#include <string>
+
+#include <dart/collision/bullet/BulletCollisionDetector.hpp>
+#include <dart/collision/dart/DARTCollisionDetector.hpp>
+#include <dart/collision/fcl/FCLCollisionDetector.hpp>
+#include <dart/collision/ode/OdeCollisionDetector.hpp>
+#include <dart/constraint/BoxedLcpConstraintSolver.hpp>
+#include <dart/constraint/ConstraintSolver.hpp>
+#include <dart/constraint/DantzigBoxedLcpSolver.hpp>
+#include <dart/constraint/PgsBoxedLcpSolver.hpp>
+#include <dart/simulation/World.hpp>
+
+#include <ignition/common/Console.hh>
+
+#include "WorldFeatures.hh"
+
+namespace ignition {
+namespace physics {
+namespace dartsim {
+
+/////////////////////////////////////////////////
+void WorldFeatures::SetWorldCollisionDetector(
+    const Identity &_id, const std::string &_collisionDetector)
+{
+  auto world = this->ReferenceInterface<dart::simulation::World>(_id);
+  auto collisionDetector =
+       world->getConstraintSolver()->getCollisionDetector();
+  if (_collisionDetector == "bullet")
+  {
+    collisionDetector = dart::collision::BulletCollisionDetector::create();
+  }
+  else if (_collisionDetector == "fcl")
+  {
+    collisionDetector = dart::collision::FCLCollisionDetector::create();
+  }
+  else if (_collisionDetector == "ode")
+  {
+    collisionDetector = dart::collision::OdeCollisionDetector::create();
+  }
+  else if (_collisionDetector == "dart")
+  {
+    collisionDetector = dart::collision::DARTCollisionDetector::create();
+  }
+  else
+  {
+    ignerr << "Collision detector [" << _collisionDetector
+           << "] is not supported, defaulting to ["
+           << collisionDetector->getType() << "]." << std::endl;
+  }
+
+  world->getConstraintSolver()->setCollisionDetector(collisionDetector);
+
+  ignmsg << "Using [" << world->getConstraintSolver()->getCollisionDetector()
+      ->getType() << "] collision detector" << std::endl;
+}
+
+/////////////////////////////////////////////////
+const std::string &WorldFeatures::GetWorldCollisionDetector(const Identity &_id)
+    const
+{
+  auto world = this->ReferenceInterface<dart::simulation::World>(_id);
+  return world->getConstraintSolver()->getCollisionDetector()->getType();
+}
+
+/////////////////////////////////////////////////
+void WorldFeatures::SetWorldSolver(const Identity &_id,
+    const std::string &_solver)
+{
+  auto world = this->ReferenceInterface<dart::simulation::World>(_id);
+
+  auto solver =
+      dynamic_cast<dart::constraint::BoxedLcpConstraintSolver *>(
+      world->getConstraintSolver());
+
+  if (!solver)
+  {
+    ignwarn << "Failed to cast constraint solver to [BoxedLcpConstraintSolver]"
+            << std::endl;
+    return;
+  }
+
+  std::shared_ptr<dart::constraint::BoxedLcpSolver> boxedSolver;
+  if (_solver == "dantzig" || _solver == "DantzigBoxedLcpSolver")
+  {
+    boxedSolver =
+        std::make_shared<dart::constraint::DantzigBoxedLcpSolver>();
+  }
+  else if (_solver == "pgs" || _solver == "PgsBoxedLcpSolver")
+  {
+    boxedSolver = std::make_shared<dart::constraint::PgsBoxedLcpSolver>();
+  }
+  else
+  {
+    ignerr << "Solver [" << _solver
+           << "] is not supported, defaulting to ["
+           << solver->getBoxedLcpSolver()->getType() << "]." << std::endl;
+  }
+
+  if (boxedSolver != nullptr)
+    solver->setBoxedLcpSolver(boxedSolver);
+
+  ignmsg << "Using [" << solver->getBoxedLcpSolver()->getType()
+         << "] solver." << std::endl;
+}
+
+/////////////////////////////////////////////////
+const std::string &WorldFeatures::GetWorldSolver(const Identity &_id) const
+{
+  auto world = this->ReferenceInterface<dart::simulation::World>(_id);
+
+  auto solver =
+      dynamic_cast<dart::constraint::BoxedLcpConstraintSolver *>(
+      world->getConstraintSolver());
+
+  if (!solver)
+  {
+    static const std::string empty{""};
+    return empty;
+  }
+
+  return solver->getBoxedLcpSolver()->getType();
+}
+
+}
+}
+}

dartsim/src/WorldFeatures.hh

@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef IGNITION_PHYSICS_DARTSIM_SRC_WORLDFEATURES_HH_
+#define IGNITION_PHYSICS_DARTSIM_SRC_WORLDFEATURES_HH_
+
+#include <string>
+
+#include <ignition/physics/World.hh>
+
+#include "Base.hh"
+
+namespace ignition {
+namespace physics {
+namespace dartsim {
+
+struct WorldFeatureList : FeatureList<
+  CollisionDetector,
+  Solver
+> { };
+
+class WorldFeatures :
+    public virtual Base,
+    public virtual Implements3d<WorldFeatureList>
+{
+  // Documentation inherited
+  public: void SetWorldCollisionDetector(
+      const Identity &_id, const std::string &_collisionDetector) override;
+
+  // Documentation inherited
+  public: const std::string &GetWorldCollisionDetector(const Identity &_id)
+      const override;
+
+  // Documentation inherited
+  public: void SetWorldSolver(const Identity &_id, const std::string &_solver)
+      override;
+
+  // Documentation inherited
+  public: const std::string &GetWorldSolver(const Identity &_id) const override;
+};
+
+}
+}
+}
+
+#endif

dartsim/src/WorldFeatures_TEST.cc

@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 3.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include <gtest/gtest.h>
+
+#include <ignition/common/Console.hh>
+#include <ignition/physics/FindFeatures.hh>
+#include <ignition/plugin/Loader.hh>
+#include <ignition/physics/RequestEngine.hh>
+
+#include <ignition/physics/ForwardStep.hh>
+#include <ignition/physics/FrameSemantics.hh>
+#include <ignition/physics/GetBoundingBox.hh>
+#include <ignition/physics/World.hh>
+#include <ignition/physics/sdf/ConstructWorld.hh>
+
+#include <sdf/Root.hh>
+#include <sdf/World.hh>
+
+#include "test/Utils.hh"
+
+struct TestFeatureList : ignition::physics::FeatureList<
+    ignition::physics::CollisionDetector,
+    ignition::physics::Solver,
+    ignition::physics::ForwardStep,
+    ignition::physics::sdf::ConstructSdfWorld,
+    ignition::physics::GetEntities
+> { };
+
+using namespace ignition;
+
+using TestEnginePtr = physics::Engine3dPtr<TestFeatureList>;
+using TestWorldPtr = physics::World3dPtr<TestFeatureList>;
+
+//////////////////////////////////////////////////
+class WorldFeaturesFixture : public ::testing::Test
+{
+  protected: void SetUp() override
+  {
+    ignition::plugin::Loader loader;
+    loader.LoadLib(dartsim_plugin_LIB);
+
+    ignition::plugin::PluginPtr dartsim =
+        loader.Instantiate("ignition::physics::dartsim::Plugin");
+
+    this->engine =
+        ignition::physics::RequestEngine3d<TestFeatureList>::From(dartsim);
+    ASSERT_NE(nullptr, this->engine);
+  }
+  protected: TestEnginePtr engine;
+};
+
+//////////////////////////////////////////////////
+TestWorldPtr LoadWorld(
+    const TestEnginePtr &_engine,
+    const std::string &_sdfFile)
+{
+  sdf::Root root;
+  const sdf::Errors errors = root.Load(_sdfFile);
+  EXPECT_TRUE(errors.empty());
+  const sdf::World *sdfWorld = root.WorldByIndex(0);
+  return _engine->ConstructWorld(*sdfWorld);
+}
+
+//////////////////////////////////////////////////
+TEST_F(WorldFeaturesFixture, CollisionDetector)
+{
+  auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/empty.sdf");
+  EXPECT_EQ("ode", world->GetCollisionDetector());
+
+  world->SetCollisionDetector("banana");
+  EXPECT_EQ("ode", world->GetCollisionDetector());
+
+  world->SetCollisionDetector("bullet");
+  EXPECT_EQ("bullet", world->GetCollisionDetector());
+
+  world->SetCollisionDetector("fcl");
+  EXPECT_EQ("fcl", world->GetCollisionDetector());
+
+  world->SetCollisionDetector("ode");
+  EXPECT_EQ("ode", world->GetCollisionDetector());
+
+  world->SetCollisionDetector("dart");
+  EXPECT_EQ("dart", world->GetCollisionDetector());
+}
+
+//////////////////////////////////////////////////
+TEST_F(WorldFeaturesFixture, Solver)
+{
+  auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/empty.sdf");
+  EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver());
+
+  world->SetSolver("banana");
+  EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver());
+
+  world->SetSolver("dantzig");
+  EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver());
+
+  world->SetSolver("pgs");
+  EXPECT_EQ("PgsBoxedLcpSolver", world->GetSolver());
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char *argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

dartsim/src/plugin.cc

@@ -27,6 +27,7 @@
 #include "SimulationFeatures.hh"
 #include "EntityManagementFeatures.hh"
 #include "FreeGroupFeatures.hh"
+#include "WorldFeatures.hh"
 
 namespace ignition {
 namespace physics {
@@ -41,8 +42,8 @@ struct DartsimFeatures : FeatureList<
   LinkFeatureList,
   SDFFeatureList,
   ShapeFeatureList,
-  SimulationFeatureList
-  // TODO(MXG): Implement more features
+  SimulationFeatureList,
+  WorldFeatureList
 > { };
 
 class Plugin :
@@ -55,7 +56,8 @@ class Plugin :
     public virtual LinkFeatures,
     public virtual SDFFeatures,
     public virtual ShapeFeatures,
-    public virtual SimulationFeatures { };
+    public virtual SimulationFeatures,
+    public virtual WorldFeatures { };
 
 IGN_PHYSICS_ADD_PLUGIN(Plugin, FeaturePolicy3d, DartsimFeatures)