Initial implementation of kinematic World

CHANGELOG.md

@@ -2,6 +2,10 @@
 
 ### 0.2.0 (201X-XX-XX)
 
+* Planner
+
+  * Added World class: [#243](https://github.com/personalrobotics/aikido/pull/243)
+
 * Build & Testing & ETC
 
   * Changed to use size_t over std::size_t: [#230](https://github.com/personalrobotics/aikido/pull/230)

include/aikido/planner.hpp

@@ -1,5 +1,6 @@
 #include "planner/PlanningResult.hpp"
 #include "planner/SnapPlanner.hpp"
+#include "planner/World.hpp"
 #include "planner/ompl/BackwardCompatibility.hpp"
 #include "planner/ompl/CRRT.hpp"
 #include "planner/ompl/CRRTConnect.hpp"

include/aikido/planner/World.hpp

@@ -0,0 +1,72 @@
+#ifndef AIKIDO_PLANNER_WORLD_HPP_
+#define AIKIDO_PLANNER_WORLD_HPP_
+
+#include <string>
+#include <dart/dart.hpp>
+
+namespace aikido {
+namespace planner {
+
+class World
+{
+public:
+  /// Construct a kinematic World.
+  World(const std::string& name = "");
+
+  virtual ~World();
+
+  /// Create a clone of this World. All Skeletons will be copied over.
+  /// \param newName Name for the cloned World
+  std::shared_ptr<World> clone(const std::string& newName = "") const;
+
+  /// Create a new World inside of a shared_ptr
+  static std::shared_ptr<World> create(const std::string& name = "");
+
+  /// Set the name of this World
+  std::string setName(const std::string& newName);
+
+  /// Get the name of this World
+  const std::string& getName() const;
+
+  /// Find a Skeleton by index
+  dart::dynamics::SkeletonPtr getSkeleton(std::size_t i) const;
+
+  /// Find a Skeleton by name
+  dart::dynamics::SkeletonPtr getSkeleton(const std::string& name) const;
+
+  /// Get the number of Skeletons
+  std::size_t getNumSkeletons() const;
+
+  /// Add a Skeleton to this World
+  std::string addSkeleton(const dart::dynamics::SkeletonPtr& skeleton);
+
+  /// Remove a Skeleton from this World
+  void removeSkeleton(const dart::dynamics::SkeletonPtr& skeleton);
+
+  // TODO: Add methods for registering callbacks?
+
+  // Get the mutex that protects the state of this World.
+  std::mutex& getMutex() const;
+
+protected:
+  /// Name of this World
+  std::string mName;
+
+  /// Skeletons in this World
+  std::vector<dart::dynamics::SkeletonPtr> mSkeletons;
+
+  mutable std::mutex mMutex;
+
+  /// NameManager for keeping track of Worlds
+  static dart::common::NameManager<World*> worldNameManager;
+
+  /// NameManager for keeping track of Skeletons
+  dart::common::NameManager<dart::dynamics::SkeletonPtr> mSkeletonNameManager;
+};
+
+typedef std::shared_ptr<World> WorldPtr;
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_WORLD_HPP_

src/planner/CMakeLists.txt

@@ -1,4 +1,6 @@
-set(sources SnapPlanner.cpp)
+set(sources
+  SnapPlanner.cpp
+  World.cpp)
 
 add_library("${PROJECT_NAME}_planner" SHARED ${sources})
 target_link_libraries("${PROJECT_NAME}_planner"

src/planner/World.cpp

@@ -0,0 +1,147 @@
+#include <aikido/planner/World.hpp>
+
+namespace aikido {
+namespace planner {
+
+dart::common::NameManager<World*> World::worldNameManager{"World", "world"};
+
+//==============================================================================
+World::World(const std::string& name)
+{
+  setName(name);
+
+  mSkeletonNameManager.setManagerName("World::Skeleton | " + mName);
+  mSkeletonNameManager.setDefaultName("skeleton");
+}
+
+//==============================================================================
+World::~World()
+{
+  World::worldNameManager.removeName(mName);
+}
+
+//==============================================================================
+WorldPtr World::clone(const std::string& newName) const
+{
+  WorldPtr worldClone(new World(newName.empty() ? mName : newName));
+
+  // Clone and add each Skeleton
+  worldClone->mSkeletons.reserve(mSkeletons.size());
+  for (std::size_t i = 0; i < mSkeletons.size(); ++i)
+  {
+    const auto clonedSkeleton = mSkeletons[i]->clone();
+    clonedSkeleton->setConfiguration(mSkeletons[i]->getConfiguration());
+    worldClone->addSkeleton(std::move(clonedSkeleton));
+  }
+
+  return worldClone;
+}
+
+//==============================================================================
+WorldPtr World::create(const std::string& name)
+{
+  WorldPtr world(new World(name));
+  return world;
+}
+
+//==============================================================================
+std::string World::setName(const std::string& newName)
+{
+  if (mName.empty())
+    mName = World::worldNameManager.issueNewNameAndAdd(newName, this);
+  else
+    mName = World::worldNameManager.changeObjectName(this, newName);
+  return mName;
+}
+
+//==============================================================================
+const std::string& World::getName() const
+{
+  return mName;
+}
+
+//==============================================================================
+dart::dynamics::SkeletonPtr World::getSkeleton(std::size_t i) const
+{
+  if (i < mSkeletons.size())
+    return mSkeletons[i];
+
+  return nullptr;
+}
+
+//==============================================================================
+dart::dynamics::SkeletonPtr World::getSkeleton(const std::string& name) const
+{
+  return mSkeletonNameManager.getObject(name);
+}
+
+//==============================================================================
+std::size_t World::getNumSkeletons() const
+{
+  return mSkeletons.size();
+}
+
+//==============================================================================
+std::string World::addSkeleton(const dart::dynamics::SkeletonPtr& skeleton)
+{
+  if (!skeleton)
+  {
+    std::cout << "[World::addSkeleton] Attempting to add a nullptr Skeleton to "
+              << "the world!" << std::endl;
+    return "";
+  }
+
+  std::lock_guard<std::mutex> lock(mMutex);
+
+  // If mSkeletons already has skeleton, then do nothing.
+  if (std::find(mSkeletons.begin(), mSkeletons.end(), skeleton)
+      != mSkeletons.end())
+  {
+    std::cout << "[World::addSkeleton] Skeleton named [" << skeleton->getName()
+              << "] is already in the world." << std::endl;
+    return skeleton->getName();
+  }
+
+  mSkeletons.push_back(skeleton);
+
+  skeleton->setName(
+      mSkeletonNameManager.issueNewNameAndAdd(skeleton->getName(), skeleton));
+
+  return skeleton->getName();
+}
+
+//==============================================================================
+void World::removeSkeleton(const dart::dynamics::SkeletonPtr& skeleton)
+{
+  if (!skeleton)
+  {
+    std::cout << "[World::removeSkeleton] Attempting to remove a nullptr "
+              << "Skeleton from the world!" << std::endl;
+    return;
+  }
+
+  std::lock_guard<std::mutex> lock(mMutex);
+
+  // If mSkeletons doesn't have skeleton, then do nothing.
+  auto skelIt = std::find(mSkeletons.begin(), mSkeletons.end(), skeleton);
+  if (skelIt == mSkeletons.end())
+  {
+    std::cout << "[World::removeSkeleton] Skeleton [" << skeleton->getName()
+              << "] is not in the world." << std::endl;
+    return;
+  }
+
+  // Remove skeleton from mSkeletons
+  mSkeletons.erase(skelIt);
+
+  mSkeletonNameManager.removeName(skeleton->getName());
+}
+
+//==============================================================================
+std::mutex& World::getMutex() const
+{
+  return mMutex;
+}
+
+} // namespace planner
+} // namespace aikido

tests/planner/CMakeLists.txt

@@ -7,3 +7,7 @@ target_link_libraries(test_SnapPlanner
   "${PROJECT_NAME}_distance"
   "${PROJECT_NAME}_trajectory"
   "${PROJECT_NAME}_planner")
+
+aikido_add_test(test_World test_World.cpp)
+target_link_libraries(test_World
+  "${PROJECT_NAME}_planner")

tests/planner/test_World.cpp

@@ -0,0 +1,93 @@
+#include <dart/dart.hpp>
+#include <gtest/gtest.h>
+#include <aikido/planner/World.hpp>
+
+using std::shared_ptr;
+using std::make_shared;
+
+class WorldTest : public ::testing::Test
+{
+public:
+  using SkeletonPtr = dart::dynamics::SkeletonPtr;
+
+  WorldTest()
+    : skel1{dart::dynamics::Skeleton::create("skel1")}
+    , skel2{dart::dynamics::Skeleton::create("skel2")}
+    , skel3{dart::dynamics::Skeleton::create("skel3")}
+    , mWorld{aikido::planner::World::create("test")}
+  {
+  }
+
+  // DART setup
+  SkeletonPtr skel1, skel2, skel3;
+  aikido::planner::WorldPtr mWorld;
+};
+
+TEST_F(WorldTest, NameTest)
+{
+  EXPECT_EQ("test", mWorld->getName());
+  mWorld->setName("different");
+  EXPECT_EQ("different", mWorld->getName());
+}
+
+TEST_F(WorldTest, AddGetRemoveSkeletons)
+{
+  // Add Skeletons
+  EXPECT_EQ(0, mWorld->getNumSkeletons());
+  mWorld->addSkeleton(skel1);
+  EXPECT_EQ(1, mWorld->getNumSkeletons());
+  mWorld->addSkeleton(skel1);
+  EXPECT_EQ(1, mWorld->getNumSkeletons());
+  mWorld->addSkeleton(skel2);
+  EXPECT_EQ(2, mWorld->getNumSkeletons());
+
+  // Getting Skeletons by index
+  EXPECT_EQ(skel1, mWorld->getSkeleton(0));
+  EXPECT_EQ(skel2, mWorld->getSkeleton(1));
+  EXPECT_TRUE(mWorld->getSkeleton(2) == nullptr);
+
+  // Getting Skeletons by name
+  EXPECT_EQ(skel1, mWorld->getSkeleton("skel1"));
+  EXPECT_EQ(skel2, mWorld->getSkeleton("skel2"));
+  EXPECT_TRUE(mWorld->getSkeleton("skel3") == nullptr);
+
+  // Remove Skeletons
+  mWorld->removeSkeleton(skel3);
+  EXPECT_EQ(2, mWorld->getNumSkeletons());
+  mWorld->removeSkeleton(skel2);
+  EXPECT_EQ(1, mWorld->getNumSkeletons());
+  mWorld->removeSkeleton(skel2);
+  EXPECT_EQ(1, mWorld->getNumSkeletons());
+  mWorld->removeSkeleton(skel1);
+  EXPECT_EQ(0, mWorld->getNumSkeletons());
+}
+
+TEST_F(WorldTest, CloningPreservesSkeletonNamesAndConfigurations)
+{
+  mWorld->addSkeleton(skel1);
+  mWorld->addSkeleton(skel2);
+  mWorld->addSkeleton(skel3);
+
+  auto clone1 = mWorld->clone();
+  EXPECT_NE(mWorld->getName(), clone1->getName());
+
+  auto clone2 = clone1->clone("clone2");
+  EXPECT_EQ("clone2", clone2->getName());
+
+  EXPECT_EQ(mWorld->getNumSkeletons(), clone1->getNumSkeletons());
+  EXPECT_EQ(mWorld->getNumSkeletons(), clone2->getNumSkeletons());
+  for (std::size_t i = 0; i < mWorld->getNumSkeletons(); ++i)
+  {
+    auto origSkeleton = mWorld->getSkeleton(i);
+    auto clonedSkeleton1 = clone1->getSkeleton(i);
+    auto clonedSkeleton2 = clone2->getSkeleton(i);
+
+    EXPECT_EQ(origSkeleton->getName(), clonedSkeleton1->getName());
+    EXPECT_EQ(origSkeleton->getName(), clonedSkeleton2->getName());
+
+    EXPECT_EQ(
+        origSkeleton->getConfiguration(), clonedSkeleton1->getConfiguration());
+    EXPECT_EQ(
+        origSkeleton->getConfiguration(), clonedSkeleton2->getConfiguration());
+  }
+}