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,33 @@
+#ifndef AIKIDO_IO_TRAJECTORY_HPP_
+#define AIKIDO_IO_TRAJECTORY_HPP_
+
+#include "aikido/trajectory/Spline.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+
+namespace aikido {
+namespace io {
+
+/// Saves a timed trajectory
+///
+/// Given a spline trajectory \c_traj, object state space and trajectory
+/// file path, saves the trajectory as a yaml file for reuse later.
+/// \param[in] trajectory Spline trajectory
+/// \param[in] skelSpace Metaskeleton of the object
+/// \param[in] savePath save path for the trajectory yaml file
+void saveTrajectory(const aikido::trajectory::Spline& trajectory,
+    const aikido::statespace::dart::MetaSkeletonStateSpacePtr& skelSpace,
+    const std::string& savePath);
+
+/// Load spline trajectory from yaml file
+///
+/// Given trajectory file and trajectory state space, this method parses
+/// the trajectory file and loads a timed trajectory for direct execution.
+/// \param[in] trajPath Spline trajectory
+/// \param[in] stateSpace Metaskeleton for the trajectory
+/// \return Loaded spline trajectory
+aikido::trajectory::UniqueSplinePtr loadSplineTrajectory(const std::string& trajPath,
+    const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace);
+} // 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], [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], 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,98 @@
+#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::MetaSkeletonStateSpacePtr;
+using aikido::trajectory::Spline;
+using aikido::trajectory::UniqueSplinePtr;
+
+namespace aikido {
+namespace io {
+
+void saveTrajectory(const aikido::trajectory::Spline& trajectory,
+    const MetaSkeletonStateSpacePtr& skelSpace, const std::string& savePath)
+{
+  std::ofstream file(savePath);
+  YAML::Emitter emitter;
+
+  statespace::ConstStateSpacePtr trajSpace = trajectory.getStateSpace();
+  emitter << YAML::BeginMap;
+  emitter << YAML::Key << "configuration" << 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 << YAML::Key << "data" << YAML::BeginSeq;
+
+  Eigen::VectorXd position(trajSpace->getDimension());
+  aikido::io::detail::encode_impl<Eigen::MatrixXd, false> convertMatrix;
+  aikido::io::detail::encode_impl<Eigen::VectorXd, false> convertVector;
+
+  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 position
+    trajSpace->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 MetaSkeletonStateSpacePtr& stateSpace)
+{
+  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 Trajectory
+      = ::aikido::common::make_unique<Spline>(stateSpace, startTime);
+
+  const YAML::Node& segments = trajFile["data"];
+  for (std::size_t i = 0; i < segments.size(); i++) {
+    const YAML::Node& segment = segments[i];
+    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*
+    aikido::statespace::StateSpace::State* startState
+        = stateSpace->allocateState();
+    stateSpace->expMap(position, startState);
+
+    Trajectory->addSegment(coefficients, duration, 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::shared_ptr;
+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("test.yml");
+  }
+
+  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;
+  SnapConfigurationToConfigurationPlanner::Result planningResult;
+};
+
+//==============================================================================
+TEST_F(SaveLoadTrajectoryTest, SavedMatchesLoaded)
+{
+  auto testable = std::make_shared<aikido::constraint::Satisfied>(stateSpace);
+
+  auto originalSmoothTrajectory = convertToSpline(*interpolated);
+  saveTrajectory(*originalSmoothTrajectory, stateSpace, "test.yml");
+
+  auto loadedSmoothTrajectory = loadSplineTrajectory("test.yml", 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));
+  }
+}

tests/trajectory/test_util.cpp

@@ -102,3 +102,4 @@ TEST_F(TrajectoryConversionTest, SuccessfulConversionToR1)
   stateSpace->logMap(sstate, stateVector);
   EXPECT_EQ(stateVector(0), (testVector(2) + testVector(1)) / 2.0);
 }
+