-
Notifications
You must be signed in to change notification settings - Fork 30
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add methods for caching and retrieving trajectories (#541)
* Add saveTrajectory method to utils * Add loadSplineTrajectory method to trajectory utils * Status: Working; Tested with simple_trajectories * Add string parameters and comments * Add tests for saving/loading trajectories * Minor formatting changes * Save order of polynomial segment in yaml file * Redesign yaml template and use emitters for writing yaml * Move caching/loading methods to aikido::io * Remove dependency on metaskeleton Use iterators over YAML segment sequences Compare dof names and spline type for deserialized trajectory * Add suggested changes * Throw runtime_error instead of assert * Format code. * Update CHANGELOG.md
- Loading branch information
Showing
8 changed files
with
303 additions
and
3 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,4 +1,5 @@ | ||
#include "io/CatkinResourceRetriever.hpp" | ||
#include "io/KinBodyParser.hpp" | ||
#include "io/trajectory.hpp" | ||
#include "io/util.hpp" | ||
#include "io/yaml.hpp" |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,125 @@ | ||
#include "aikido/io/trajectory.hpp" | ||
|
||
#include <algorithm> | ||
#include <fstream> | ||
#include <iostream> | ||
#include <boost/program_options.hpp> | ||
#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<const MetaSkeletonStateSpace>( | ||
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<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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,118 @@ | ||
#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)); | ||
} | ||
} |