Add suggested changes

personalrobotics · Aug 27, 2019 · aefbc97 · aefbc97
1 parent 372ad2a
commit aefbc97
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 23 deletions.
diff --git a/include/aikido/io/trajectory.hpp b/include/aikido/io/trajectory.hpp
@@ -36,7 +36,7 @@ void saveTrajectory(const aikido::trajectory::Spline& trajectory,
 /// \param[in] metaSkeletonStateSpace MetaskeletonStateSpace for the trajectory
 /// \return Loaded spline trajectory
 aikido::trajectory::UniqueSplinePtr loadSplineTrajectory(const std::string& trajPath,
-    const aikido::statespace::dart::MetaSkeletonStateSpacePtr& metaSkeletonStateSpace);
+    const aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr& metaSkeletonStateSpace);
 } // namespace io
 } // namespace aikido
 

diff --git a/src/io/trajectory.cpp b/src/io/trajectory.cpp
@@ -13,7 +13,7 @@
 using aikido::statespace::ConstStateSpacePtr;
 using aikido::statespace::StateSpacePtr;
 using aikido::statespace::dart::MetaSkeletonStateSpace;
-using aikido::statespace::dart::MetaSkeletonStateSpacePtr;
+using aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr;
 using aikido::trajectory::Spline;
 using aikido::trajectory::UniqueSplinePtr;
 
@@ -28,6 +28,8 @@ void saveTrajectory(const aikido::trajectory::Spline& trajectory,
 
   auto skelSpace = std::dynamic_pointer_cast<const MetaSkeletonStateSpace>
       (trajectory.getStateSpace());
+  assert(skelSpace != nullptr);
+
   emitter << YAML::BeginMap;
   emitter << YAML::Key << "configuration";
   emitter << YAML::BeginMap;
@@ -70,31 +72,30 @@ void saveTrajectory(const aikido::trajectory::Spline& trajectory,
 
 //==============================================================================
 UniqueSplinePtr loadSplineTrajectory(
-    const std::string& trajPath, const MetaSkeletonStateSpacePtr& stateSpace)
+    const std::string& trajPath,
+    const ConstMetaSkeletonStateSpacePtr& metaSkeletonStateSpace)
 {
   YAML::Node trajFile = YAML::LoadFile(trajPath);
   const YAML::Node& config = trajFile["configuration"];
 
   double startTime = config["start_time"].as<double>();
   std::vector<std::string> dofs = config["dofs"].as<std::vector<std::string>>();
-  std::string trajType = config["type"].as<std::string>();
-
-  auto paramDofs = stateSpace->getProperties().getDofNames();
-  bool is_equal = false;
-  is_equal = std::equal(dofs.begin(), dofs.end(), paramDofs.begin());
-
-  if (!is_equal)
+  auto paramDofs = metaSkeletonStateSpace->getProperties().getDofNames();
+  if (!std::equal(dofs.begin(), dofs.end(), paramDofs.begin()))
+  {
     throw std::runtime_error("Dof names should be same");
+  }
 
+  std::string trajType = config["type"].as<std::string>();
   if (trajType.compare("spline"))
   {
     throw std::runtime_error("Trajectory type should be spline");
   }
 
   auto trajectory
-      = ::aikido::common::make_unique<Spline>(stateSpace, startTime);
+      = ::aikido::common::make_unique<Spline>(metaSkeletonStateSpace, startTime);
   aikido::statespace::StateSpace::State* startState
-        = stateSpace->allocateState();
+        = metaSkeletonStateSpace->allocateState();
 
   const YAML::Node& segments = trajFile["data"];
   for (YAML::const_iterator it = segments.begin(); it != segments.end(); ++it)
@@ -105,12 +106,12 @@ UniqueSplinePtr loadSplineTrajectory(
     Eigen::VectorXd position = segment["start_state"].as<Eigen::VectorXd>();
 
     // Convert position Eigen vector to StateSpace::State*
-    stateSpace->expMap(position, startState);
+    metaSkeletonStateSpace->expMap(position, startState);
 
     trajectory->addSegment(coefficients, duration, startState);
   }
 
-  stateSpace->freeState(startState);
+  metaSkeletonStateSpace->freeState(startState);
   return trajectory;
 }
 

diff --git a/tests/io/test_trajectory_io.cpp b/tests/io/test_trajectory_io.cpp
@@ -17,7 +17,6 @@
 #include <aikido/trajectory/util.hpp>
 #include "../constraint/MockConstraints.hpp"
 
-using std::shared_ptr;
 using std::make_shared;
 using aikido::io::loadSplineTrajectory;
 using aikido::io::saveTrajectory;
@@ -76,12 +75,12 @@ class SaveLoadTrajectoryTest : public ::testing::Test
   SkeletonPtr skel;
   std::pair<JointPtr, BodyNodePtr> jn_bn;
 
-  shared_ptr<MetaSkeletonStateSpace> stateSpace;
-  shared_ptr<ScopedState> startState;
-  shared_ptr<ScopedState> goalState;
-  shared_ptr<PassingConstraint> passingConstraint;
-  shared_ptr<GeodesicInterpolator> interpolator;
-  shared_ptr<Interpolated> interpolated;
+  std::shared_ptr<MetaSkeletonStateSpace> stateSpace;
+  std::shared_ptr<ScopedState> startState;
+  std::shared_ptr<ScopedState> goalState;
+  std::shared_ptr<PassingConstraint> passingConstraint;
+  std::shared_ptr<GeodesicInterpolator> interpolator;
+  std::shared_ptr<Interpolated> interpolated;
   SnapConfigurationToConfigurationPlanner::Result planningResult;
 };
 
@@ -91,9 +90,10 @@ TEST_F(SaveLoadTrajectoryTest, SavedMatchesLoaded)
   auto testable = std::make_shared<aikido::constraint::Satisfied>(stateSpace);
 
   auto originalSmoothTrajectory = convertToSpline(*interpolated);
-  saveTrajectory(*originalSmoothTrajectory, "test.yml");
+  std::string trajFileName = "test.yml";
+  saveTrajectory(*originalSmoothTrajectory, trajFileName);
 
-  auto loadedSmoothTrajectory = loadSplineTrajectory("test.yml", stateSpace);
+  auto loadedSmoothTrajectory = loadSplineTrajectory(trajFileName, stateSpace);
 
   EXPECT_EQ(originalSmoothTrajectory->getDuration(),
       loadedSmoothTrajectory->getDuration());