Add methods for caching and retrieving trajectories

include/aikido/io.hpp

@@ -1,4 +1,5 @@
 #include "io/CatkinResourceRetriever.hpp"
 #include "io/KinBodyParser.hpp"
+#include "io/trajectory.hpp"
 #include "io/util.hpp"
 #include "io/yaml.hpp"

include/aikido/io/trajectory.hpp

@@ -0,0 +1,43 @@
+#ifndef AIKIDO_IO_TRAJECTORY_HPP_
+#define AIKIDO_IO_TRAJECTORY_HPP_
+
+#include "aikido/trajectory/Spline.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+
+/// Format of serialized trajectory in YAML
+///
+/// configuration:
+///   start_time: 0
+///   dofs: ["/right/j1", "/right/j2", ...]
+///   type: spline
+///   spline:
+///     order: 3
+/// data:
+///   - coefficients: [1, 2, ...]
+///     duration: 0.1875
+///     start_state: [3, 4, ...]
+///   - coefficients: [5, 6, ...]
+///     duration: 0.1875
+///     start_state: [7, 8, ...]
+
+namespace aikido {
+namespace io {
+
+/// Serializes a spline trajectory to YAML.
+///
+/// \param[in] trajectory Spline trajectory
+/// \param[in] savePath save path for the trajectory yaml file
+void saveTrajectory(const aikido::trajectory::Spline& trajectory,
+    const std::string& savePath);
+
+/// Deserializes a spline trajectory from YAML.
+///
+/// \param[in] trajPath 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::ConstMetaSkeletonStateSpacePtr& metaSkeletonStateSpace);
+} // namespace io
+} // namespace aikido
+
+#endif // AIKIDO_IO_TRAJECTORY_HPP_

src/CMakeLists.txt

@@ -5,14 +5,14 @@ add_subdirectory("external/hauser_parabolic_smoother")
 add_subdirectory("external/kunz_retimer")
 
 add_subdirectory("common")     # boost, dart
-add_subdirectory("io")         # [common], boost, dart, tinyxml2, yaml-cpp
 add_subdirectory("statespace") # dart
-add_subdirectory("perception") # [io], boost, dart, yaml-cpp, geometry_msgs, roscpp, std_msgs, visualization_msgs
 add_subdirectory("distance")   # [statespace], dart
 add_subdirectory("trajectory") # [common], [distance], [statespace]
 add_subdirectory("constraint") # [common], [statespace]
 add_subdirectory("planner")    # [external], [common], [statespace], [trajectory], [constraint], [distance], dart, ompl
 add_subdirectory("rviz")       # [constraint], [planner], boost, dart, roscpp, geometry_msgs, interactive_markers, std_msgs, visualization_msgs, libmicrohttpd
 add_subdirectory("control")    # [statespace], [trajectory]
+add_subdirectory("io")         # [common], [trajectory], boost, dart, tinyxml2, yaml-cpp
+add_subdirectory("perception") # [io], boost, dart, yaml-cpp, geometry_msgs, roscpp, std_msgs, visualization_msgs
 add_subdirectory("robot")      # [common], [io], [statespace], [trajectory], [constraint], [planner], [control]
 #add_subdirectory("python")     # everything

src/io/CMakeLists.txt

@@ -28,6 +28,7 @@ check_cxx_source_compiles(
 set(sources
   CatkinResourceRetriever.cpp
   KinBodyParser.cpp
+  trajectory.cpp
   yaml.cpp
   util.cpp
 )
@@ -44,6 +45,7 @@ target_include_directories("${PROJECT_NAME}_io" SYSTEM
 target_link_libraries("${PROJECT_NAME}_io"
   PUBLIC
     "${PROJECT_NAME}_common"
+    "${PROJECT_NAME}_trajectory"
     ${Boost_FILESYSTEM_LIBRARY}
     ${DART_LIBRARIES}
     ${YAMLCPP_LIBRARIES}
@@ -67,6 +69,6 @@ endif()
 
 add_component(${PROJECT_NAME} io)
 add_component_targets(${PROJECT_NAME} io "${PROJECT_NAME}_io")
-add_component_dependencies(${PROJECT_NAME} io common)
+add_component_dependencies(${PROJECT_NAME} io common trajectory)
 
 clang_format_add_sources(${sources})

src/io/trajectory.cpp

@@ -0,0 +1,119 @@
+#include "aikido/io/trajectory.hpp"
+
+#include <algorithm>
+#include <fstream>
+#include <iostream>
+#include <boost/program_options.hpp>
+#include "aikido/common/Spline.hpp"
+#include "aikido/io/yaml.hpp"
+#include "aikido/io/detail/yaml_extension.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+
+using aikido::statespace::ConstStateSpacePtr;
+using aikido::statespace::StateSpacePtr;
+using aikido::statespace::dart::MetaSkeletonStateSpace;
+using aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr;
+using aikido::trajectory::Spline;
+using aikido::trajectory::UniqueSplinePtr;
+
+namespace aikido {
+namespace io {
+
+void saveTrajectory(const aikido::trajectory::Spline& trajectory,
+    const std::string& savePath)
+{
+  std::ofstream file(savePath);
+  YAML::Emitter emitter;
+
+  auto skelSpace = std::dynamic_pointer_cast<const MetaSkeletonStateSpace>
+      (trajectory.getStateSpace());
+  assert(skelSpace != nullptr);
+
+  emitter << YAML::BeginMap;
+  emitter << YAML::Key << "configuration";
+  emitter << YAML::BeginMap;
+  emitter << YAML::Key << "start_time" << YAML::Value << trajectory.getStartTime();
+  emitter << YAML::Key << "dofs" << YAML::Flow << skelSpace->getProperties().getDofNames();
+  emitter << YAML::Key << "type" << YAML::Value << "spline";
+  emitter << YAML::Key << "spline";
+  emitter << YAML::BeginMap;
+  emitter << YAML::Key << "order" << YAML::Value << trajectory.getNumDerivatives();
+  emitter << YAML::EndMap;
+  emitter << YAML::EndMap;
+
+  Eigen::VectorXd position(skelSpace->getDimension());
+  aikido::io::detail::encode_impl<Eigen::MatrixXd, false> convertMatrix;
+  aikido::io::detail::encode_impl<Eigen::VectorXd, false> convertVector;
+
+  emitter << YAML::Key << "data";
+  emitter << YAML::BeginSeq;
+  for (std::size_t i = 0; i < trajectory.getNumSegments(); ++i)
+  {
+    auto startState = trajectory.getSegmentStartState(i);
+    double duration = trajectory.getSegmentDuration(i);
+    auto segmentCoeff = trajectory.getSegmentCoefficients(i);
+
+    // Convert start state to Eigen vector
+    skelSpace->logMap(startState, position);
+
+    emitter << YAML::BeginMap;
+    emitter << YAML::Key << "coefficients" << YAML::Flow
+        << convertMatrix.encode(segmentCoeff);
+    emitter << YAML::Key << "duration" << YAML::Value << duration;
+    emitter << YAML::Key << "start_state" << YAML::Flow
+        << convertVector.encode(position);
+    emitter << YAML::EndMap;
+  }
+
+  emitter << YAML::EndSeq << YAML::EndMap;
+  file << emitter.c_str();
+}
+
+//==============================================================================
+UniqueSplinePtr loadSplineTrajectory(
+    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>>();
+  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>(metaSkeletonStateSpace, startTime);
+  aikido::statespace::StateSpace::State* startState
+        = metaSkeletonStateSpace->allocateState();
+
+  const YAML::Node& segments = trajFile["data"];
+  for (YAML::const_iterator it = segments.begin(); it != segments.end(); ++it)
+  {
+    const YAML::Node& segment = *it;
+    Eigen::MatrixXd coefficients = segment["coefficients"].as<Eigen::MatrixXd>();
+    double duration = segment["duration"].as<double>();
+    Eigen::VectorXd position = segment["start_state"].as<Eigen::VectorXd>();
+
+    // Convert position Eigen vector to StateSpace::State*
+    metaSkeletonStateSpace->expMap(position, startState);
+
+    trajectory->addSegment(coefficients, duration, startState);
+  }
+
+  metaSkeletonStateSpace->freeState(startState);
+  return trajectory;
+}
+
+} // namespace io
+} // namespace aikido

tests/io/CMakeLists.txt

@@ -43,3 +43,10 @@ target_compile_definitions(test_loadSkeletonFromURDF
 
 aikido_add_test(test_yaml_extension test_yaml_extension.cpp)
 target_link_libraries(test_yaml_extension "${PROJECT_NAME}_io")
+
+aikido_add_test(test_trajectory_io test_trajectory_io.cpp)
+target_link_libraries(test_trajectory_io
+  "${PROJECT_NAME}_constraint"
+  "${PROJECT_NAME}_io"
+  "${PROJECT_NAME}_planner"
+  "${PROJECT_NAME}_trajectory")

tests/io/test_trajectory_io.cpp

@@ -0,0 +1,116 @@
+#include "aikido/io/trajectory.hpp"
+
+#include <tuple>
+#include <dart/dart.hpp>
+#include <gtest/gtest.h>
+#include <aikido/constraint/Satisfied.hpp>
+#include <aikido/constraint/Testable.hpp>
+#include <aikido/io/yaml.hpp>
+#include <aikido/planner/ConfigurationToConfiguration.hpp>
+#include <aikido/planner/SnapConfigurationToConfigurationPlanner.hpp>
+#include <aikido/statespace/GeodesicInterpolator.hpp>
+#include <aikido/statespace/SO2.hpp>
+#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
+
+#include <aikido/trajectory/Interpolated.hpp>
+#include <aikido/trajectory/Spline.hpp>
+#include <aikido/trajectory/util.hpp>
+#include "../constraint/MockConstraints.hpp"
+
+using std::make_shared;
+using aikido::io::loadSplineTrajectory;
+using aikido::io::saveTrajectory;
+using aikido::trajectory::Interpolated;
+using aikido::planner::ConfigurationToConfiguration;
+using aikido::planner::SnapConfigurationToConfigurationPlanner;
+using aikido::statespace::ConstStateSpacePtr;
+using aikido::trajectory::convertToSpline;
+
+//==============================================================================
+class SaveLoadTrajectoryTest : public ::testing::Test
+{
+public:
+  using MetaSkeletonStateSpace
+      = aikido::statespace::dart::MetaSkeletonStateSpace;
+  using SO2 = aikido::statespace::SO2;
+  using GeodesicInterpolator = aikido::statespace::GeodesicInterpolator;
+  using ScopedState = MetaSkeletonStateSpace::ScopedState;
+
+  using BodyNodePtr = dart::dynamics::BodyNodePtr;
+  using JointPtr = dart::dynamics::JointPtr;
+  using SkeletonPtr = dart::dynamics::SkeletonPtr;
+
+  SaveLoadTrajectoryTest()
+    : skel{dart::dynamics::Skeleton::create("skel")}
+    , jn_bn{skel->createJointAndBodyNodePair<dart::dynamics::RevoluteJoint>()}
+    , stateSpace{make_shared<MetaSkeletonStateSpace>(skel.get())}
+    , startState{make_shared<ScopedState>(stateSpace->createState())}
+    , goalState{make_shared<ScopedState>(stateSpace->createState())}
+    , passingConstraint{make_shared<PassingConstraint>(stateSpace)}
+    , interpolator(make_shared<GeodesicInterpolator>(stateSpace))
+    , interpolated(make_shared<Interpolated>(stateSpace, interpolator))
+  {
+    // Do nothing
+  }
+
+  void SetUp() override
+  {
+    stateSpace->getState(skel.get(), *startState);
+    skel->setPosition(0, 2.0);
+    stateSpace->setState(skel.get(), *goalState);
+
+    auto problem = ConfigurationToConfiguration(
+        stateSpace, *startState, *goalState, passingConstraint);
+    auto planner = std::make_shared<SnapConfigurationToConfigurationPlanner>(
+        stateSpace, interpolator);
+    auto traj = planner->plan(problem, &planningResult);
+    interpolated = std::dynamic_pointer_cast<Interpolated>(traj);
+  }
+
+  ~SaveLoadTrajectoryTest()
+  {
+    std::remove(trajFileName.c_str());
+  }
+
+  SkeletonPtr skel;
+  std::pair<JointPtr, BodyNodePtr> jn_bn;
+
+  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;
+  std::string trajFileName = "test.yml";
+  SnapConfigurationToConfigurationPlanner::Result planningResult;
+};
+
+//==============================================================================
+TEST_F(SaveLoadTrajectoryTest, SavedMatchesLoaded)
+{
+  auto testable = std::make_shared<aikido::constraint::Satisfied>(stateSpace);
+
+  auto originalSmoothTrajectory = convertToSpline(*interpolated);
+  saveTrajectory(*originalSmoothTrajectory, trajFileName);
+
+  auto loadedSmoothTrajectory = loadSplineTrajectory(trajFileName, stateSpace);
+
+  EXPECT_EQ(originalSmoothTrajectory->getDuration(),
+      loadedSmoothTrajectory->getDuration());
+  for (std::size_t i = 0; i < loadedSmoothTrajectory->getNumSegments(); ++i)
+  {
+    Eigen::MatrixXd originalCoefficients
+        = originalSmoothTrajectory->getSegmentCoefficients(i);
+    Eigen::MatrixXd loadedCoefficients
+        = loadedSmoothTrajectory->getSegmentCoefficients(i);
+    EXPECT_TRUE(loadedCoefficients.isApprox(originalCoefficients)) << loadedCoefficients;
+
+    Eigen::VectorXd originalPosition(stateSpace->getDimension());
+    Eigen::VectorXd loadedPosition(stateSpace->getDimension());
+    stateSpace->logMap(
+        originalSmoothTrajectory->getSegmentStartState(i), originalPosition);
+    stateSpace->logMap(
+        loadedSmoothTrajectory->getSegmentStartState(i), loadedPosition);
+    EXPECT_TRUE(loadedPosition.isApprox(originalPosition));
+  }
+}