diff --git a/CHANGELOG.md b/CHANGELOG.md index 46e058ea06..e2b3509f4b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -86,6 +86,7 @@ * IO * Added loadSkeletonFromURDF convenience function: [#388](https://github.com/personalrobotics/aikido/pull/388), [#401](https://github.com/personalrobotics/aikido/pull/401) + * Added methods for caching and retrieving trajectories: [#541](https://github.com/personalrobotics/aikido/pull/541) * Build & Testing & ETC diff --git a/include/aikido/io.hpp b/include/aikido/io.hpp index eb4d576706..e9dac9a7e3 100644 --- a/include/aikido/io.hpp +++ b/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" diff --git a/include/aikido/io/trajectory.hpp b/include/aikido/io/trajectory.hpp new file mode 100644 index 0000000000..829ca377f0 --- /dev/null +++ b/include/aikido/io/trajectory.hpp @@ -0,0 +1,46 @@ +#ifndef AIKIDO_IO_TRAJECTORY_HPP_ +#define AIKIDO_IO_TRAJECTORY_HPP_ + +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" +#include "aikido/trajectory/Spline.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_ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 172b6f76fe..5ffbeaf71a 100644 --- a/src/CMakeLists.txt +++ b/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 diff --git a/src/io/CMakeLists.txt b/src/io/CMakeLists.txt index 4047153b5f..296023eeda 100644 --- a/src/io/CMakeLists.txt +++ b/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}) diff --git a/src/io/trajectory.cpp b/src/io/trajectory.cpp new file mode 100644 index 0000000000..77bff93228 --- /dev/null +++ b/src/io/trajectory.cpp @@ -0,0 +1,125 @@ +#include "aikido/io/trajectory.hpp" + +#include +#include +#include +#include +#include "aikido/common/Spline.hpp" +#include "aikido/io/detail/yaml_extension.hpp" +#include "aikido/io/yaml.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( + trajectory.getStateSpace()); + if (!skelSpace) + throw std::runtime_error( + "Trajectory state space is not MetaSkeletonStateSpace"); + + 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 convertMatrix; + aikido::io::detail::encode_impl 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(); + std::vector dofs = config["dofs"].as>(); + 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(); + if (trajType.compare("spline")) + { + throw std::runtime_error("Trajectory type should be spline"); + } + + auto trajectory = ::aikido::common::make_unique( + 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(); + double duration = segment["duration"].as(); + Eigen::VectorXd position = segment["start_state"].as(); + + // 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 diff --git a/tests/io/CMakeLists.txt b/tests/io/CMakeLists.txt index bb443e8594..d9e7b0a7f4 100644 --- a/tests/io/CMakeLists.txt +++ b/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") diff --git a/tests/io/test_trajectory_io.cpp b/tests/io/test_trajectory_io.cpp new file mode 100644 index 0000000000..9b2bfa6631 --- /dev/null +++ b/tests/io/test_trajectory_io.cpp @@ -0,0 +1,118 @@ +#include "aikido/io/trajectory.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#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()} + , stateSpace{make_shared(skel.get())} + , startState{make_shared(stateSpace->createState())} + , goalState{make_shared(stateSpace->createState())} + , passingConstraint{make_shared(stateSpace)} + , interpolator(make_shared(stateSpace)) + , interpolated(make_shared(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( + stateSpace, interpolator); + auto traj = planner->plan(problem, &planningResult); + interpolated = std::dynamic_pointer_cast(traj); + } + + ~SaveLoadTrajectoryTest() + { + std::remove(trajFileName.c_str()); + } + + SkeletonPtr skel; + std::pair jn_bn; + + std::shared_ptr stateSpace; + std::shared_ptr startState; + std::shared_ptr goalState; + std::shared_ptr passingConstraint; + std::shared_ptr interpolator; + std::shared_ptr interpolated; + std::string trajFileName = "test.yml"; + SnapConfigurationToConfigurationPlanner::Result planningResult; +}; + +//============================================================================== +TEST_F(SaveLoadTrajectoryTest, SavedMatchesLoaded) +{ + auto testable = std::make_shared(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)); + } +}