From fe6ba16de63568c3d98dc6ccb043a0e26254da0f Mon Sep 17 00:00:00 2001 From: Aditya Vamsikrishna Mandalika Date: Fri, 15 Feb 2019 22:07:48 -0800 Subject: [PATCH] SO2 to R1 conversion for postprocessing (#496) * Convert SO2 to R1 during smoothing * Remove whitespaces * Add conversion * code format Co-Authored-By: aditya-vk * code format Co-Authored-By: aditya-vk * address PR comments * propogate changes * change iter to const Co-Authored-By: aditya-vk * cleanup * make format * add enum to handle both cases * use enum to support both types * formatting * remove support for multi-dof * modify kunzretimer test * complete kunz retimer test rewrite * parabolic smoother * parabolic timer * all tests converted * make format * yet another test * make format * Remove hacky SO(2) to R1 conversion. Improve documentation. [skip ci] * Update CHANGELOG. * Return unique_ptr rather than shared_ptr. * Add TODO to clean up toR1JointTrajectory usage. --- CHANGELOG.md | 1 + include/aikido/control/ros/Conversions.hpp | 4 +- .../aikido/statespace/CartesianProduct.hpp | 2 + include/aikido/trajectory/util.hpp | 14 ++ src/control/ros/Conversions.cpp | 136 ++++++++++++--- src/planner/kunzretimer/KunzRetimer.cpp | 69 ++++++-- src/planner/parabolic/ParabolicUtil.cpp | 76 ++++++-- .../vectorfield/VectorFieldPlanner.cpp | 19 +- src/trajectory/util.cpp | 128 +++++++++----- .../planner/kunzretimer/test_KunzRetimer.cpp | 150 +++++++++------- .../test_KunzRetimerPostProcessor.cpp | 49 ++++-- .../parabolic/test_ParabolicSmoother.cpp | 165 ++++++++++-------- .../planner/parabolic/test_ParabolicTimer.cpp | 123 +++++++------ .../parabolic/test_SmoothPostProcessor.cpp | 111 +++++++----- .../parabolic/test_TimePostProcessor.cpp | 46 +++-- tests/trajectory/CMakeLists.txt | 5 + tests/trajectory/test_SplineTrajectory.cpp | 76 +++++--- tests/trajectory/test_util.cpp | 104 +++++++++++ 18 files changed, 893 insertions(+), 385 deletions(-) create mode 100644 tests/trajectory/test_util.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index b6db27b8b5..35d4fd8df8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -62,6 +62,7 @@ * Added a kinodynamic timer that generates a time-optimal smooth trajectory without completely stopping at each waypoint: [#443](https://github.com/personalrobotics/aikido/pull/443) * Fixed segmentation fault on 32-bit machines in vector-field planner: [#459](https://github.com/personalrobotics/aikido/pull/459) * Updated interface to OMPL planners to follow the style of the new refactored planning API: [#466](https://github.com/personalrobotics/aikido/pull/466) + * Added convenience function for converting SO(2) trajectories to R1 trajectories, support for postprocessing SO(2) trajectories: [#496](https://github.com/personalrobotics/aikido/pull/496) * Robot diff --git a/include/aikido/control/ros/Conversions.hpp b/include/aikido/control/ros/Conversions.hpp index f205420678..cb8b83acc7 100644 --- a/include/aikido/control/ros/Conversions.hpp +++ b/include/aikido/control/ros/Conversions.hpp @@ -4,8 +4,8 @@ #include #include #include -#include -#include +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" +#include "aikido/trajectory/Spline.hpp" namespace aikido { namespace control { diff --git a/include/aikido/statespace/CartesianProduct.hpp b/include/aikido/statespace/CartesianProduct.hpp index 7d07bdf7ec..fa7fefd1ea 100644 --- a/include/aikido/statespace/CartesianProduct.hpp +++ b/include/aikido/statespace/CartesianProduct.hpp @@ -7,6 +7,8 @@ namespace aikido { namespace statespace { +AIKIDO_DECLARE_POINTERS(CartesianProduct) + // Defined in detail/CartesianProduct.hpp template class CompoundStateHandle; diff --git a/include/aikido/trajectory/util.hpp b/include/aikido/trajectory/util.hpp index 0a796f470c..02b3ef6674 100644 --- a/include/aikido/trajectory/util.hpp +++ b/include/aikido/trajectory/util.hpp @@ -89,6 +89,20 @@ double findTimeOfClosestStateOnTrajectory( UniqueSplinePtr createPartialTrajectory( const Spline& traj, double partialStartTime); +/// Converts an interpolated trajectory from a Cartesian product space of SO(2) +/// and R1 joints to a Cartesian product space of strictly R1 joints. +/// +/// \param[in] trajectory Trajectory to be converted. +/// \return Converted trajectory. +UniqueInterpolatedPtr toR1JointTrajectory(const Interpolated& trajectory); + +/// Converts a spline trajectory from a Cartesian product space of SO(2) and R1 +/// joints to a Cartesian product space of strictly R1 joints. +/// +/// \param[in] trajectory Trajectory to be converted. +/// \return Converted trajectory. +UniqueSplinePtr toR1JointTrajectory(const Spline& trajectory); + } // namespace trajectory } // namespace aikido diff --git a/src/control/ros/Conversions.cpp b/src/control/ros/Conversions.cpp index 3538b1374a..467b76bea2 100644 --- a/src/control/ros/Conversions.cpp +++ b/src/control/ros/Conversions.cpp @@ -1,14 +1,15 @@ -#include +#include "aikido/control/ros/Conversions.hpp" #include #include #include -#include -#include -#include -#include -#include +#include "aikido/common/Spline.hpp" +#include "aikido/common/StepSequence.hpp" +#include "aikido/control/ros/Conversions.hpp" +#include "aikido/statespace/dart/RnJoint.hpp" +#include "aikido/statespace/dart/SO2Joint.hpp" +using aikido::statespace::CartesianProduct; using aikido::statespace::dart::MetaSkeletonStateSpace; using SplineTrajectory = aikido::trajectory::Spline; using aikido::statespace::dart::R1Joint; @@ -19,11 +20,102 @@ namespace control { namespace ros { namespace { +/// Reorder the elements of a vector according to an index map. +/// \param[in] indexMap Map denoting the reordering between in and out indices. +/// \param[in] inVector Vector to reorder. +/// \param[out] outVector Reordered vector. void reorder( const std::vector>& indexMap, const Eigen::VectorXd& inVector, Eigen::VectorXd& outVector); +/// Checks if a vector is valid. +/// \param[in] name Name. Provides context to what the vector is representing. +/// \param[in] values The std::vector to check. +/// \param[in] expectedLength Expected size of the vector. +/// \param[in] isRequired True if the vector is expected to contain entries. +/// \param[out] output Converted Eigen vector. +void checkVector( + const std::string& name, + const std::vector& values, + std::size_t expectedLength, + bool isRequired, + Eigen::VectorXd& output); + +/// Fits a polynomial between two states [t, position, velocity, acceleration]. +/// \param[in] currTime Start time. +/// \param[in] currPosition Start position. +/// \param[in] currVelocity Start Velocity. +/// \param[in] currAcceleration Start Acceleration. +/// \param[in] nextTime End time. +/// \param[in] nextPosition End Position. +/// \param[in] nextVelocity End Velocity. +/// \param[in] nextAcceleration End Acceleration. +/// \param[in] numCoefficients Degree of the polynomial to fit. +Eigen::MatrixXd fitPolynomial( + double currTime, + const Eigen::VectorXd& currPosition, + const Eigen::VectorXd& currVelocity, + const Eigen::VectorXd& currAcceleration, + double nextTime, + const Eigen::VectorXd& nextPosition, + const Eigen::VectorXd& nextVelocity, + const Eigen::VectorXd& nextAcceleration, + std::size_t numCoefficients); + +/// Extract a state on the joint trajectory given an index. +/// \param[in] trajectory Joint trajectory to extract points from. +/// \param[in] index Index of the point on the trajectory to extract. +/// \param[out] positions Extracted positions. +/// \param[in] positionsRequired True if positions should be extracted. +/// \param[out] velocities Extracted velocities corresponding to \c positions. +/// \param[in] velocitiesRequired True if velocities should be extracted. +/// \param[out] accelerations Extracted accelerations corresponding to \c +/// positions. +/// \param[in] accelerationsRequired True if accelerations should be extracted. +/// \param[in] indexMap Map denoting the correct ordering of trajectory data. +/// This is required in case the trajectory's joint indexing is different than +/// metaskeleton joint indexing. +/// \param[in] unspecifiedJoints Joints whose data is not required. Assumed to +/// be static at the current position. +/// \param[in] startPositions Start positions of the joints. +void extractJointTrajectoryPoint( + const trajectory_msgs::JointTrajectory& _trajectory, + std::size_t index, + std::size_t numDofs, + Eigen::VectorXd& positions, + bool positionsRequired, + Eigen::VectorXd& velocities, + bool velocitiesRequired, + Eigen::VectorXd& accelerations, + bool accelerationsRequired, + const std::vector>& indexMap, + const std::vector& unspecifiedJoints, + const Eigen::VectorXd& startPositions); + +/// Extract a state on the trajectory given a timepoint. +/// \param[in] space MetaSkeletonStateSpace of the trajectory. +/// \param[in] trajectory Trajectory to extract point from. +/// \param[in] timeFromStart Timepoint to extract trajectory point at. +/// \param[out] waypoint The extracted trajectory point. +void extractTrajectoryPoint( + const std::shared_ptr& space, + const aikido::trajectory::ConstTrajectoryPtr& trajectory, + double timeFromStart, + trajectory_msgs::JointTrajectoryPoint& waypoint); + +//============================================================================== +void reorder( + const std::vector>& indexMap, + const Eigen::VectorXd& inVector, + Eigen::VectorXd& outVector) +{ + assert(indexMap.size() == static_cast(inVector.size())); + outVector.resize(inVector.size()); + for (const auto& index : indexMap) + outVector[index.second] = inVector[index.first]; +} + //============================================================================== void checkVector( const std::string& _name, @@ -195,19 +287,6 @@ void extractTrajectoryPoint( } } -//============================================================================== -// The rows of inVector is reordered in outVector. -void reorder( - const std::vector>& indexMap, - const Eigen::VectorXd& inVector, - Eigen::VectorXd& outVector) -{ - assert(indexMap.size() == static_cast(inVector.size())); - outVector.resize(inVector.size()); - for (auto index : indexMap) - outVector[index.second] = inVector[index.first]; -} - } // namespace //============================================================================== @@ -374,8 +453,18 @@ std::unique_ptr toSplineJointTrajectory( numCoefficients = 2; // linear // Convert the ROS trajectory message to an Aikido spline. - std::unique_ptr trajectory{new SplineTrajectory{space}}; - auto currState = space->createState(); + // Add a segment to the trajectory. + std::vector subspaces; + for (std::size_t citer = 0; citer < space->getDimension(); ++citer) + { + subspaces.emplace_back(std::make_shared()); + } + + auto compoundSpace + = std::make_shared(subspaces); + std::unique_ptr trajectory{ + new SplineTrajectory{compoundSpace}}; + auto currState = compoundSpace->createState(); const auto& waypoints = jointTrajectory.points; for (std::size_t iWaypoint = 1; iWaypoint < waypoints.size(); ++iWaypoint) @@ -411,8 +500,7 @@ std::unique_ptr toSplineJointTrajectory( nextAcceleration, numCoefficients); - // Add a segment to the trajectory. - space->convertPositionsToState(currPosition, currState); + compoundSpace->expMap(currPosition, currState); trajectory->addSegment(segmentCoefficients, segmentDuration, currState); // Advance to the next segment. @@ -497,7 +585,7 @@ trajectory_msgs::JointTrajectory toRosJointTrajectory( // Evaluate trajectory at each timestep and insert it into jointTrajectory jointTrajectory.points.reserve(numWaypoints); - for (const auto timeFromStart : timeSequence) + for (const auto& timeFromStart : timeSequence) { trajectory_msgs::JointTrajectoryPoint waypoint; diff --git a/src/planner/kunzretimer/KunzRetimer.cpp b/src/planner/kunzretimer/KunzRetimer.cpp index 80ee451a6a..f6f26ed9c7 100644 --- a/src/planner/kunzretimer/KunzRetimer.cpp +++ b/src/planner/kunzretimer/KunzRetimer.cpp @@ -1,11 +1,22 @@ #include "aikido/planner/kunzretimer/KunzRetimer.hpp" #include -#include -#include +#include "aikido/common/Spline.hpp" +#include "aikido/common/StepSequence.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" +#include "aikido/trajectory/Interpolated.hpp" +#include "aikido/trajectory/Spline.hpp" +#include "aikido/trajectory/util.hpp" #include "Path.h" #include "Trajectory.h" +using dart::common::make_unique; +using aikido::statespace::dart::MetaSkeletonStateSpace; +using aikido::statespace::ConstStateSpacePtr; +using aikido::trajectory::toR1JointTrajectory; +using aikido::trajectory::ConstSplinePtr; +using aikido::trajectory::ConstInterpolatedPtr; + namespace aikido { namespace planner { namespace kunzretimer { @@ -15,17 +26,33 @@ namespace detail { std::unique_ptr convertToKunzPath( const aikido::trajectory::Interpolated& traj, double maxDeviation) { - std::list waypoints; auto stateSpace = traj.getStateSpace(); + auto metaSkeletonStateSpace + = std::dynamic_pointer_cast(stateSpace); + + const aikido::trajectory::Interpolated* trajectory = &traj; + + ConstInterpolatedPtr r1Trajectory; + if (metaSkeletonStateSpace) + { + r1Trajectory = toR1JointTrajectory(traj); + stateSpace = r1Trajectory->getStateSpace(); + trajectory = r1Trajectory.get(); + } + + // TODO(brian): debug + // auto trajectory = toR1JointTrajectory(traj); + // auto stateSpace = trajectory->getStateSpace(); + + std::list waypoints; Eigen::VectorXd tmpVec(stateSpace->getDimension()); - for (std::size_t i = 0; i < traj.getNumWaypoints(); i++) + for (std::size_t i = 0; i < trajectory->getNumWaypoints(); i++) { - auto tmpState = traj.getWaypoint(i); + auto tmpState = trajectory->getWaypoint(i); stateSpace->logMap(tmpState, tmpVec); waypoints.push_back(tmpVec); } - - auto path = ::dart::common::make_unique(waypoints, maxDeviation); + auto path = make_unique(waypoints, maxDeviation); return path; } @@ -33,18 +60,37 @@ std::unique_ptr convertToKunzPath( std::unique_ptr convertToKunzPath( const aikido::trajectory::Spline& traj, double maxDeviation) { - std::list waypoints; auto stateSpace = traj.getStateSpace(); + auto metaSkeletonStateSpace + = std::dynamic_pointer_cast(stateSpace); + + const aikido::trajectory::Spline* trajectory = &traj; + + ConstSplinePtr r1Trajectory; + if (metaSkeletonStateSpace) + { + r1Trajectory = toR1JointTrajectory(traj); + stateSpace = r1Trajectory->getStateSpace(); + trajectory = r1Trajectory.get(); + } + + // TODO(brian): debug + // auto trajectory = toR1JointTrajectory(traj); + // auto stateSpace = trajectory->getStateSpace(); + + std::list waypoints; + + stateSpace = trajectory->getStateSpace(); Eigen::VectorXd tmpVec(stateSpace->getDimension()); auto tmpState = stateSpace->createState(); - for (std::size_t i = 0; i < traj.getNumWaypoints(); i++) + for (std::size_t i = 0; i < trajectory->getNumWaypoints(); i++) { - traj.getWaypoint(i, tmpState); + trajectory->getWaypoint(i, tmpState); stateSpace->logMap(tmpState, tmpVec); waypoints.push_back(tmpVec); } - auto path = ::dart::common::make_unique(waypoints, maxDeviation); + auto path = make_unique(waypoints, maxDeviation); return path; } @@ -133,7 +179,6 @@ std::unique_ptr computeKunzTiming( } double startTime = inputTrajectory.getStartTime(); - auto path = detail::convertToKunzPath(inputTrajectory, maxDeviation); Trajectory trajectory(*path, maxVelocity, maxAcceleration, timeStep); return detail::convertToSpline(trajectory, stateSpace, timeStep, startTime); diff --git a/src/planner/parabolic/ParabolicUtil.cpp b/src/planner/parabolic/ParabolicUtil.cpp index f004cf1a21..28ed7d5190 100644 --- a/src/planner/parabolic/ParabolicUtil.cpp +++ b/src/planner/parabolic/ParabolicUtil.cpp @@ -5,15 +5,22 @@ #include #include -#include -#include -#include -#include +#include "aikido/common/Spline.hpp" +#include "aikido/statespace/GeodesicInterpolator.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" +#include "aikido/trajectory/Interpolated.hpp" +#include "aikido/trajectory/Spline.hpp" +#include "aikido/trajectory/util.hpp" #include "DynamicPath.h" using Eigen::Vector2d; using aikido::statespace::StateSpace; +using aikido::statespace::dart::MetaSkeletonStateSpace; +using aikido::trajectory::toR1JointTrajectory; +using aikido::trajectory::ConstInterpolatedPtr; +using aikido::trajectory::ConstSplinePtr; +using dart::common::make_unique; using CubicSplineProblem = aikido::common::SplineProblem; @@ -23,6 +30,7 @@ namespace planner { namespace parabolic { namespace detail { +//============================================================================== ParabolicRamp::Vector toVector(const Eigen::VectorXd& _x) { ParabolicRamp::Vector output(_x.size()); @@ -33,6 +41,7 @@ ParabolicRamp::Vector toVector(const Eigen::VectorXd& _x) return output; } +//============================================================================== Eigen::VectorXd toEigen(const ParabolicRamp::Vector& _x) { Eigen::VectorXd output(_x.size()); @@ -43,6 +52,7 @@ Eigen::VectorXd toEigen(const ParabolicRamp::Vector& _x) return output; } +//============================================================================== void evaluateAtTime( const ParabolicRamp::DynamicPath& _path, double _t, @@ -58,6 +68,7 @@ void evaluateAtTime( _velocity = toEigen(velocityVector); } +//============================================================================== std::unique_ptr convertToSpline( const ParabolicRamp::DynamicPath& _inputPath, double _startTime, @@ -91,9 +102,8 @@ std::unique_ptr convertToSpline( Eigen::VectorXd positionPrev, velocityPrev; evaluateAtTime(_inputPath, timePrev, positionPrev, velocityPrev); - auto _outputTrajectory - = ::dart::common::make_unique( - _stateSpace, timePrev + _startTime); + auto _outputTrajectory = make_unique( + _stateSpace, timePrev + _startTime); auto segmentStartState = _stateSpace->createState(); for (const auto timeCurr : transitionTimes) @@ -124,15 +134,34 @@ std::unique_ptr convertToSpline( return _outputTrajectory; } +//============================================================================== std::unique_ptr convertToDynamicPath( const aikido::trajectory::Spline& _inputTrajectory, const Eigen::VectorXd& _maxVelocity, const Eigen::VectorXd& _maxAcceleration, bool _preserveWaypointVelocity) { - const auto stateSpace = _inputTrajectory.getStateSpace(); + auto stateSpace = _inputTrajectory.getStateSpace(); + auto metaSkeletonStateSpace + = std::dynamic_pointer_cast(stateSpace); + + const aikido::trajectory::Spline* trajectory = &_inputTrajectory; + + ConstSplinePtr r1Trajectory; + if (metaSkeletonStateSpace) + { + r1Trajectory = toR1JointTrajectory(_inputTrajectory); + stateSpace = r1Trajectory->getStateSpace(); + trajectory = r1Trajectory.get(); + } + const auto numWaypoints = _inputTrajectory.getNumWaypoints(); + // TODO(brian): debug + // auto trajectory = toR1JointTrajectory(_inputTrajectory); + // auto stateSpace = trajectory->getStateSpace(); + // const auto numWaypoints = trajectory->getNumWaypoints(); + std::vector milestones; std::vector velocities; milestones.reserve(numWaypoints); @@ -143,16 +172,16 @@ std::unique_ptr convertToDynamicPath( for (std::size_t iwaypoint = 0; iwaypoint < numWaypoints; ++iwaypoint) { auto currentState = stateSpace->createState(); - _inputTrajectory.getWaypoint(iwaypoint, currentState); + trajectory->getWaypoint(iwaypoint, currentState); stateSpace->logMap(currentState, currVec); milestones.emplace_back(toVector(currVec)); - _inputTrajectory.getWaypointDerivative(iwaypoint, 1, tangentVector); + trajectory->getWaypointDerivative(iwaypoint, 1, tangentVector); velocities.emplace_back(toVector(tangentVector)); } - auto outputPath = ::dart::common::make_unique(); + auto outputPath = make_unique(); outputPath->Init(toVector(_maxVelocity), toVector(_maxAcceleration)); if (_preserveWaypointVelocity) { @@ -167,14 +196,33 @@ std::unique_ptr convertToDynamicPath( return outputPath; } +//============================================================================== std::unique_ptr convertToDynamicPath( const aikido::trajectory::Interpolated& _inputTrajectory, const Eigen::VectorXd& _maxVelocity, const Eigen::VectorXd& _maxAcceleration) { - const auto stateSpace = _inputTrajectory.getStateSpace(); + auto stateSpace = _inputTrajectory.getStateSpace(); + auto metaSkeletonStateSpace + = std::dynamic_pointer_cast(stateSpace); + const auto numWaypoints = _inputTrajectory.getNumWaypoints(); + const aikido::trajectory::Interpolated* trajectory = &_inputTrajectory; + + ConstInterpolatedPtr r1Trajectory; + if (metaSkeletonStateSpace) + { + r1Trajectory = toR1JointTrajectory(_inputTrajectory); + stateSpace = r1Trajectory->getStateSpace(); + trajectory = r1Trajectory.get(); + } + + // TODO(brian): debug + // auto trajectory = toR1JointTrajectory(_inputTrajectory); + // auto stateSpace = trajectory->getStateSpace(); + // const auto numWaypoints = trajectory->getNumWaypoints(); + std::vector milestones; std::vector velocities; milestones.reserve(numWaypoints); @@ -184,12 +232,12 @@ std::unique_ptr convertToDynamicPath( for (std::size_t iwaypoint = 0; iwaypoint < numWaypoints; ++iwaypoint) { - auto currentState = _inputTrajectory.getWaypoint(iwaypoint); + auto currentState = trajectory->getWaypoint(iwaypoint); stateSpace->logMap(currentState, currVec); milestones.emplace_back(toVector(currVec)); } - auto outputPath = ::dart::common::make_unique(); + auto outputPath = make_unique(); outputPath->Init(toVector(_maxVelocity), toVector(_maxAcceleration)); outputPath->SetMilestones(milestones); if (!outputPath->IsValid()) diff --git a/src/planner/vectorfield/VectorFieldPlanner.cpp b/src/planner/vectorfield/VectorFieldPlanner.cpp index 20494ff9f3..36ea94499d 100644 --- a/src/planner/vectorfield/VectorFieldPlanner.cpp +++ b/src/planner/vectorfield/VectorFieldPlanner.cpp @@ -1,12 +1,14 @@ +#include "aikido/planner/vectorfield/VectorFieldPlanner.hpp" + #include -#include -#include -#include -#include -#include -#include -#include -#include + +#include "aikido/constraint/TestableIntersection.hpp" +#include "aikido/constraint/dart/JointStateSpaceHelpers.hpp" +#include "aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp" +#include "aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp" +#include "aikido/planner/vectorfield/VectorFieldUtil.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp" +#include "aikido/trajectory/Spline.hpp" #include "detail/VectorFieldIntegrator.hpp" #include "detail/VectorFieldPlannerExceptions.hpp" @@ -180,7 +182,6 @@ std::unique_ptr planToEndEffectorOffset( compoundConstraint->addConstraint(constraint); compoundConstraint->addConstraint( constraint::dart::createTestableBounds(stateSpace)); - return followVectorField( *vectorfield, startState, diff --git a/src/trajectory/util.cpp b/src/trajectory/util.cpp index 4519c9324e..133d7127e3 100644 --- a/src/trajectory/util.cpp +++ b/src/trajectory/util.cpp @@ -11,11 +11,19 @@ #include "aikido/statespace/CartesianProduct.hpp" #include "aikido/statespace/Rn.hpp" #include "aikido/statespace/SO2.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" #include "aikido/trajectory/Interpolated.hpp" using aikido::statespace::R; +using aikido::statespace::R1; using aikido::statespace::SO2; using aikido::statespace::CartesianProduct; +using aikido::statespace::ConstStateSpacePtr; +using aikido::statespace::GeodesicInterpolator; +using aikido::statespace::StateSpacePtr; +using aikido::statespace::dart::MetaSkeletonStateSpace; +using aikido::statespace::dart::MetaSkeletonStateSpacePtr; +using dart::common::make_unique; using Eigen::Vector2d; using LinearSplineProblem @@ -30,36 +38,8 @@ namespace { bool checkStateSpace(const statespace::StateSpace* _stateSpace) { - // TODO(JS): Generalize Rn for arbitrary N. - if (dynamic_cast*>(_stateSpace) != nullptr) - { - return true; - } - else if (dynamic_cast*>(_stateSpace) != nullptr) - { - return true; - } - else if (dynamic_cast*>(_stateSpace) != nullptr) - { - return true; - } - else if (dynamic_cast*>(_stateSpace) != nullptr) - { - return true; - } - else if (dynamic_cast*>(_stateSpace) != nullptr) - { - return true; - } - else if (dynamic_cast*>(_stateSpace) != nullptr) - { - return true; - } - else if (dynamic_cast*>(_stateSpace) != nullptr) - { - return true; - } - else if (dynamic_cast*>(_stateSpace) != nullptr) + // Only supports single-DOF joint spaces, namely R1 and SO2. + if (dynamic_cast(_stateSpace) != nullptr) { return true; } @@ -83,7 +63,7 @@ bool checkStateSpace(const statespace::StateSpace* _stateSpace) } } -} // (anonymous) namespace +} // namespace //============================================================================== UniqueSplinePtr convertToSpline(const Interpolated& inputTrajectory) @@ -103,14 +83,13 @@ UniqueSplinePtr convertToSpline(const Interpolated& inputTrajectory) if (!checkStateSpace(stateSpace.get())) throw std::invalid_argument( - "convertToSpline only supports Rn, " - "SO2, and CartesianProducts consisting of those types."); + "convertToSpline only supports Rn and SO2 joint spaces"); if (numWaypoints == 0) throw std::invalid_argument("Trajectory is empty."); - auto outputTrajectory = ::dart::common::make_unique( - stateSpace, inputTrajectory.getStartTime()); + auto outputTrajectory + = make_unique(stateSpace, inputTrajectory.getStartTime()); Eigen::VectorXd currentVec, nextVec; for (std::size_t iwaypoint = 0; iwaypoint < numWaypoints - 1; ++iwaypoint) @@ -144,8 +123,8 @@ UniqueInterpolatedPtr convertToInterpolated( const Spline& traj, statespace::ConstInterpolatorPtr interpolator) { auto stateSpace = traj.getStateSpace(); - auto outputTrajectory = ::dart::common::make_unique( - stateSpace, std::move(interpolator)); + auto outputTrajectory + = make_unique(stateSpace, std::move(interpolator)); auto state = stateSpace->createState(); for (std::size_t i = 0; i < traj.getNumWaypoints(); ++i) @@ -168,7 +147,7 @@ UniqueInterpolatedPtr concatenate( if (traj1.getInterpolator() != traj2.getInterpolator()) throw std::runtime_error("Interpolator mismatch"); - auto outputTrajectory = ::dart::common::make_unique( + auto outputTrajectory = make_unique( traj1.getStateSpace(), traj1.getInterpolator()); if (traj1.getNumWaypoints() > 1u) { @@ -253,8 +232,7 @@ UniqueSplinePtr createPartialTrajectory( const auto stateSpace = traj.getStateSpace(); const int dimension = static_cast(stateSpace->getDimension()); - auto outputTrajectory - = ::dart::common::make_unique(stateSpace, traj.getStartTime()); + auto outputTrajectory = make_unique(stateSpace, traj.getStartTime()); double currSegmentStartTime = traj.getStartTime(); double currSegmentEndTime = currSegmentStartTime; @@ -315,5 +293,75 @@ UniqueSplinePtr createPartialTrajectory( return outputTrajectory; } +//============================================================================== +UniqueInterpolatedPtr toR1JointTrajectory(const Interpolated& trajectory) +{ + if (!checkStateSpace(trajectory.getStateSpace().get())) + throw std::invalid_argument( + "toR1JointTrajectory only supports R1 and SO2 joint spaces"); + + auto interpolator = std::dynamic_pointer_cast( + trajectory.getInterpolator()); + if (!interpolator) + throw std::invalid_argument( + "The interpolator of trajectory should be a GeodesicInterpolator"); + + // Create new trajectory space. + auto space = trajectory.getStateSpace(); + std::vector subspaces; + for (std::size_t i = 0; i < space->getDimension(); ++i) + subspaces.emplace_back(std::make_shared()); + + auto rSpace = std::make_shared(subspaces); + auto rInterpolator = std::make_shared(rSpace); + auto rTrajectory = make_unique(rSpace, rInterpolator); + + Eigen::VectorXd sourceVector(space->getDimension()); + auto sourceState = rSpace->createState(); + + // Add the first waypoint + space->logMap(trajectory.getWaypoint(0), sourceVector); + rSpace->expMap(sourceVector, sourceState); + rTrajectory->addWaypoint(0, sourceState); + + auto tangentState = rSpace->createState(); + auto targetState = rSpace->createState(); + + // Add the remaining waypoints + for (std::size_t i = 0; i < trajectory.getNumWaypoints() - 1; ++i) + { + const auto tangentVector = interpolator->getTangentVector( + trajectory.getWaypoint(i), trajectory.getWaypoint(i + 1)); + + rSpace->expMap(sourceVector, sourceState); + rSpace->expMap(tangentVector, tangentState); + rSpace->compose(sourceState, tangentState, targetState); + + rTrajectory->addWaypoint(i + 1, targetState); + rSpace->logMap(targetState, sourceVector); + } + + return rTrajectory; +} + +//============================================================================== +UniqueSplinePtr toR1JointTrajectory(const Spline& trajectory) +{ + if (!checkStateSpace(trajectory.getStateSpace().get())) + throw std::invalid_argument( + "toR1JointTrajectory only supports R1 and SO2 joint spaces"); + + auto space = trajectory.getStateSpace(); + aikido::statespace::ConstInterpolatorPtr interpolator + = std::make_shared(space); + + ConstInterpolatedPtr interpolatedTrajectory + = std::move(convertToInterpolated(trajectory, interpolator)); + auto r1JointTrajectory = toR1JointTrajectory(*interpolatedTrajectory); + auto splineTrajectory = convertToSpline(*r1JointTrajectory); + + return splineTrajectory; +} + } // namespace trajectory } // namespace aikido diff --git a/tests/planner/kunzretimer/test_KunzRetimer.cpp b/tests/planner/kunzretimer/test_KunzRetimer.cpp index ffab63f0a5..df26fd3d45 100644 --- a/tests/planner/kunzretimer/test_KunzRetimer.cpp +++ b/tests/planner/kunzretimer/test_KunzRetimer.cpp @@ -12,7 +12,7 @@ using Eigen::Vector2d; using Eigen::Vector3d; using aikido::trajectory::Interpolated; using aikido::statespace::GeodesicInterpolator; -using aikido::statespace::R2; +using aikido::statespace::R1; using aikido::statespace::CartesianProduct; using aikido::statespace::SO2; using aikido::statespace::StateSpacePtr; @@ -28,7 +28,11 @@ class KunzRetimerTests : public ::testing::Test protected: void SetUp() override { - mStateSpace = std::make_shared(); + std::vector subspaces; + for (std::size_t i = 0; i < 2; ++i) + subspaces.emplace_back(std::make_shared()); + mStateSpace = std::make_shared(subspaces); + mMaxVelocity = Eigen::Vector2d(1., 1.); mMaxAcceleration = Eigen::Vector2d(2., 2.); @@ -36,15 +40,18 @@ class KunzRetimerTests : public ::testing::Test mStraightLine = std::make_shared(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); + Eigen::VectorXd positions(2); - state.setValue(Vector2d(1., 2.)); + positions << 1.0, 2.0; + mStateSpace->expMap(positions, state); mStraightLine->addWaypoint(0., state); - state.setValue(Vector2d(3., 4.)); + positions << 3.0, 4.0; + mStateSpace->expMap(positions, state); mStraightLine->addWaypoint(1., state); } - std::shared_ptr mStateSpace; + std::shared_ptr mStateSpace; Eigen::Vector2d mMaxVelocity; Eigen::Vector2d mMaxAcceleration; @@ -52,6 +59,12 @@ class KunzRetimerTests : public ::testing::Test std::shared_ptr mStraightLine; }; +TEST_F(KunzRetimerTests, SupportedCartesianProduct_DoesNotThrow) +{ + EXPECT_NO_THROW( + { computeKunzTiming(*mStraightLine, mMaxVelocity, mMaxAcceleration); }); +} + TEST_F(KunzRetimerTests, MaxVelocityIsZero_Throws) { Vector2d zeroMaxVelocity(1., 0.); @@ -95,28 +108,32 @@ TEST_F(KunzRetimerTests, StartsAtNonZeroTime) Interpolated inputTrajectory(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); - // This is the same test as StraightLine_TriangularProfile, except that the // trajectory starts at a non-zero time. - state.setValue(Vector2d(1., 2.)); + Eigen::VectorXd positions(2); + positions << 1, 2; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(1., state); - state.setValue(Vector2d(2., 3.)); + positions << 2, 3; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(3., state); auto timedTrajectory = computeKunzTiming( inputTrajectory, Vector2d::Constant(2.), Vector2d::Constant(1.)); - EXPECT_FALSE(timedTrajectory == nullptr) << "Trajectory timing failed"; timedTrajectory->evaluate(1., state); - EXPECT_EIGEN_EQUAL(Vector2d(1.0, 2.0), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(1.0, 2.0), positions, 1e-6); timedTrajectory->evaluate(2., state); - EXPECT_EIGEN_EQUAL(Vector2d(1.5, 2.5), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(1.5, 2.5), positions, 1e-6); timedTrajectory->evaluate(3., state); - EXPECT_EIGEN_EQUAL(Vector2d(2.0, 3.0), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(2.0, 3.0), positions, 1e-6); } TEST_F(KunzRetimerTests, InterploatedSplineEquivalence) @@ -128,10 +145,15 @@ TEST_F(KunzRetimerTests, InterploatedSplineEquivalence) // This is the same test as StraightLine_TriangularProfile, except that the // trajectory starts at a non-zero time. - state.setValue(Vector2d(1., 2.)); + Eigen::VectorXd positions(2); + Eigen::VectorXd positions2(2); + + positions << 1, 2; + mStateSpace->expMap(positions, state); interpolated.addWaypoint(1., state); - state.setValue(Vector2d(2., 3.)); + positions << 2, 3; + mStateSpace->expMap(positions, state); interpolated.addWaypoint(3., state); auto spline = convertToSpline(interpolated); @@ -143,15 +165,21 @@ TEST_F(KunzRetimerTests, InterploatedSplineEquivalence) timedInterpolated->evaluate(1., state); timedSpline->evaluate(1., state2); - EXPECT_EIGEN_EQUAL(state2.getValue(), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_EIGEN_EQUAL(positions2, positions, 1e-6); timedInterpolated->evaluate(2., state); timedSpline->evaluate(2., state2); - EXPECT_EIGEN_EQUAL(state2.getValue(), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_EIGEN_EQUAL(positions2, positions, 1e-6); timedInterpolated->evaluate(3., state); timedSpline->evaluate(3., state2); - EXPECT_EIGEN_EQUAL(state2.getValue(), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_EIGEN_EQUAL(positions2, positions, 1e-6); } TEST_F(KunzRetimerTests, StraightLine_TriangularProfile) @@ -159,6 +187,7 @@ TEST_F(KunzRetimerTests, StraightLine_TriangularProfile) Interpolated inputTrajectory(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); + Eigen::VectorXd positions(2); Eigen::VectorXd tangentVector; double maxDeviation = 1e-2; @@ -166,10 +195,12 @@ TEST_F(KunzRetimerTests, StraightLine_TriangularProfile) // The optimal timing of this trajectory should be a triangle centered at t = // 1that accelerates at 1 rad/s^2 for 1 s, then deaccelerates at -1 rad/s^2 // for 1 s. This corresponds to moving each axis through 2 rad. - state.setValue(Vector2d(1., 2.)); + positions << 1, 2; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(0., state); - state.setValue(Vector2d(2., 3.)); + positions << 2, 3; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(2., state); auto timedTrajectory = computeKunzTiming( @@ -186,13 +217,16 @@ TEST_F(KunzRetimerTests, StraightLine_TriangularProfile) // Position. timedTrajectory->evaluate(0., state); - EXPECT_EIGEN_EQUAL(Vector2d(1.0, 2.0), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(1.0, 2.0), positions, 1e-6); timedTrajectory->evaluate(1., state); - EXPECT_EIGEN_EQUAL(Vector2d(1.5, 2.5), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(1.5, 2.5), positions, 1e-6); timedTrajectory->evaluate(2., state); - EXPECT_EIGEN_EQUAL(Vector2d(2.0, 3.0), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(2.0, 3.0), positions, 1e-6); // Velocity timedTrajectory->evaluateDerivative(0.5, 1, tangentVector); @@ -217,16 +251,19 @@ TEST_F(KunzRetimerTests, StraightLine_TrapezoidalProfile) Interpolated inputTrajectory(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); + Eigen::VectorXd positions(2); Eigen::VectorXd tangentVector; // The optimal timing of this trajectory should be a trapezoid that: // - accelerates at 1 rad/s^2 for 1 s // - coasts at 1 m/s for 1 s // - deaccelerates at -1 rad/s^2 for 1 s - state.setValue(Vector2d(1., 2.)); + positions << 1, 2; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(0., state); - state.setValue(Vector2d(3., 4.)); + positions << 3, 4; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(2., state); double maxDeviation = 1e-2; @@ -245,16 +282,20 @@ TEST_F(KunzRetimerTests, StraightLine_TrapezoidalProfile) // Position. timedTrajectory->evaluate(0., state); - EXPECT_EIGEN_EQUAL(Vector2d(1.0, 2.0), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(1.0, 2.0), positions, 1e-6); timedTrajectory->evaluate(1., state); - EXPECT_EIGEN_EQUAL(Vector2d(1.5, 2.5), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(1.5, 2.5), positions, 1e-6); timedTrajectory->evaluate(2., state); - EXPECT_EIGEN_EQUAL(Vector2d(2.5, 3.5), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(2.5, 3.5), positions, 1e-6); timedTrajectory->evaluate(3., state); - EXPECT_EIGEN_EQUAL(Vector2d(3.0, 4.0), state.getValue(), 1e-6); + mStateSpace->logMap(state, positions); + EXPECT_EIGEN_EQUAL(Vector2d(3.0, 4.0), positions, 1e-6); // Velocity timedTrajectory->evaluateDerivative(0.5, 1, tangentVector); @@ -293,6 +334,7 @@ TEST_F(KunzRetimerTests, StraightLine_DifferentAccelerationLimits) Interpolated inputTrajectory(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); + Eigen::VectorXd positions(2); Eigen::VectorXd tangentVector; // The optimal timing of this trajectory should be a trapezoid that: @@ -303,10 +345,12 @@ TEST_F(KunzRetimerTests, StraightLine_DifferentAccelerationLimits) // Note that the second dimension of the state space could result in a faster // timing by executing a triangular velocity profile. This is not possible // because the first dimension has a lower acceleration limit. - state.setValue(Vector2d(1., 2.)); + positions << 1, 2; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(0., state); - state.setValue(Vector2d(3., 4.)); + positions << 3, 4; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(2., state); auto timedTrajectory @@ -317,35 +361,13 @@ TEST_F(KunzRetimerTests, StraightLine_DifferentAccelerationLimits) EXPECT_NEAR(3., timedTrajectory->getDuration(), durationTolerance); } -TEST_F(KunzRetimerTests, SupportedCartesianProduct_DoesNotThrow) -{ - auto stateSpace = std::make_shared( - std::vector{ - std::make_shared(), std::make_shared(), - }); - auto state = stateSpace->createState(); - - std::shared_ptr interpolator - = std::make_shared(stateSpace); - - Interpolated inputTrajectory(stateSpace, interpolator); - - state.getSubStateHandle(0).setValue(Vector2d::Zero()); - state.getSubStateHandle(1).fromAngle(0.); - inputTrajectory.addWaypoint(0., state); - - state.getSubStateHandle(0).setValue(Vector2d::Zero()); - state.getSubStateHandle(1).fromAngle(M_PI_2); - inputTrajectory.addWaypoint(1., state); - - EXPECT_NO_THROW({ - computeKunzTiming(inputTrajectory, Vector3d::Ones(), Vector3d::Ones()); - }); -} - TEST_F(KunzRetimerTests, timingArbitraryMultipleWaypoints) { - auto stateSpace = std::make_shared>(); + std::vector subspaces; + for (std::size_t i = 0; i < 4; ++i) + subspaces.emplace_back(std::make_shared()); + auto stateSpace = std::make_shared(subspaces); + auto interpolator = std::make_shared(stateSpace); Interpolated inputTrajectory(stateSpace, interpolator); @@ -353,19 +375,23 @@ TEST_F(KunzRetimerTests, timingArbitraryMultipleWaypoints) auto state = stateSpace->createState(); waypoint << 1427.0, 368.0, 690.0, 90.0; - state.setValue(waypoint); + stateSpace->expMap(waypoint, state); inputTrajectory.addWaypoint(0., state); + waypoint << 1427.0, 368.0, 790.0, 90.0; - state.setValue(waypoint); + stateSpace->expMap(waypoint, state); inputTrajectory.addWaypoint(1., state); + waypoint << 952.0, 433.0, 1051.0, 90.0; - state.setValue(waypoint); + stateSpace->expMap(waypoint, state); inputTrajectory.addWaypoint(2., state); + waypoint << 452.5, 533.0, 1051.0, 90.0; - state.setValue(waypoint); + stateSpace->expMap(waypoint, state); inputTrajectory.addWaypoint(3., state); + waypoint << 452.5, 533.0, 951.0, 90.0; - state.setValue(waypoint); + stateSpace->expMap(waypoint, state); inputTrajectory.addWaypoint(4., state); Eigen::VectorXd maxVelocities(4); diff --git a/tests/planner/kunzretimer/test_KunzRetimerPostProcessor.cpp b/tests/planner/kunzretimer/test_KunzRetimerPostProcessor.cpp index 11a5f4fb2b..1cb5136f42 100644 --- a/tests/planner/kunzretimer/test_KunzRetimerPostProcessor.cpp +++ b/tests/planner/kunzretimer/test_KunzRetimerPostProcessor.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -9,8 +10,10 @@ using Eigen::Vector2d; using aikido::trajectory::Interpolated; +using aikido::statespace::CartesianProduct; +using aikido::statespace::ConstStateSpacePtr; using aikido::statespace::GeodesicInterpolator; -using aikido::statespace::R2; +using aikido::statespace::R1; using aikido::trajectory::convertToSpline; using aikido::planner::kunzretimer::KunzRetimer; @@ -20,7 +23,10 @@ class KunzRetimerPostProcessorTests : public ::testing::Test void SetUp() override { mRng = aikido::common::RNGWrapper(0); - mStateSpace = std::make_shared(); + std::vector subspaces; + for (std::size_t i = 0; i < 2; ++i) + subspaces.emplace_back(std::make_shared()); + mStateSpace = std::make_shared(subspaces); mInterpolator = std::make_shared(mStateSpace); // set default parameters @@ -29,7 +35,7 @@ class KunzRetimerPostProcessorTests : public ::testing::Test } aikido::common::RNGWrapper mRng; - std::shared_ptr mStateSpace; + std::shared_ptr mStateSpace; std::shared_ptr mInterpolator; double mMaxDeviation; @@ -44,15 +50,18 @@ TEST_F(KunzRetimerPostProcessorTests, testTime) Interpolated inputTrajectory(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); + Eigen::VectorXd positions(2); Eigen::VectorXd tangentVector; // The optimal timing of this trajectory should be a triangle centered at t = // 1that accelerates at 1 rad/s^2 for 1 s, then deaccelerates at -1 rad/s^2 // for 1 s. This corresponds to moving each axis through 2 rad. - state.setValue(Vector2d(1., 2.)); + positions << 1, 2; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(0., state); - state.setValue(Vector2d(2., 3.)); + positions << 2, 3; + mStateSpace->expMap(positions, state); inputTrajectory.addWaypoint(2., state); auto timedTrajectory @@ -64,13 +73,16 @@ TEST_F(KunzRetimerPostProcessorTests, testTime) // Position. timedTrajectory->evaluate(0., state); - EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(positions)); timedTrajectory->evaluate(1., state); - EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(positions)); timedTrajectory->evaluate(2., state); - EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(positions)); // Velocity timedTrajectory->evaluateDerivative(0.5, 1, tangentVector); @@ -100,14 +112,19 @@ TEST_F(KunzRetimerPostProcessorTests, testSplineTiming) Interpolated interpolated(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); + Eigen::VectorXd positions(2); + auto state2 = mStateSpace->createState(); + Eigen::VectorXd positions2(2); // This is the same test as StraightLine_TriangularProfile, except that the // trajectory starts at a non-zero time. - state.setValue(Vector2d(1., 2.)); + positions << 1, 2; + mStateSpace->expMap(positions, state); interpolated.addWaypoint(1., state); - state.setValue(Vector2d(2., 3.)); + positions << 2, 3; + mStateSpace->expMap(positions, state); interpolated.addWaypoint(3., state); auto spline = convertToSpline(interpolated); @@ -118,13 +135,19 @@ TEST_F(KunzRetimerPostProcessorTests, testSplineTiming) timedInterpolated->evaluate(1., state); timedSpline->evaluate(1., state2); - EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_TRUE(positions2.isApprox(positions)); timedInterpolated->evaluate(2., state); timedSpline->evaluate(2., state2); - EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_TRUE(positions2.isApprox(positions)); timedInterpolated->evaluate(3., state); timedSpline->evaluate(3., state2); - EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_TRUE(positions2.isApprox(positions)); } diff --git a/tests/planner/parabolic/test_ParabolicSmoother.cpp b/tests/planner/parabolic/test_ParabolicSmoother.cpp index 3cd0e1172f..a28ccab827 100644 --- a/tests/planner/parabolic/test_ParabolicSmoother.cpp +++ b/tests/planner/parabolic/test_ParabolicSmoother.cpp @@ -6,20 +6,15 @@ #include #include #include -#include -#include #include #include "eigen_tests.hpp" using Eigen::Vector2d; -using Eigen::Vector3d; using aikido::trajectory::Interpolated; +using aikido::statespace::ConstStateSpacePtr; using aikido::statespace::GeodesicInterpolator; -using aikido::statespace::R2; +using aikido::statespace::R1; using aikido::statespace::CartesianProduct; -using aikido::statespace::SO2; -using aikido::statespace::SO3; -using aikido::statespace::StateSpacePtr; using aikido::constraint::Satisfied; using aikido::trajectory::convertToSpline; using aikido::planner::parabolic::computeParabolicTiming; @@ -35,7 +30,10 @@ class ParabolicSmootherTests : public ::testing::Test void SetUp() override { mRng = aikido::common::RNGWrapper(0); - mStateSpace = std::make_shared(); + std::vector subspaces; + for (std::size_t i = 0; i < 2; ++i) + subspaces.emplace_back(std::make_shared()); + mStateSpace = std::make_shared(subspaces); mMaxVelocity = Eigen::Vector2d(20., 20.); mMaxAcceleration = Eigen::Vector2d(10., 10.); @@ -52,10 +50,10 @@ class ParabolicSmootherTests : public ::testing::Test mStraightLine = std::make_shared(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); - Vector2d p1(1., 2.), p2(3., 4.); - state.setValue(p1); + Vector2d p1(1, 2), p2(3, 4); + mStateSpace->expMap(p1, state); mStraightLine->addWaypoint(0., state); - state.setValue(p2); + mStateSpace->expMap(p2, state); mStraightLine->addWaypoint(1., state); return (p2 - p1).norm(); @@ -67,19 +65,20 @@ class ParabolicSmootherTests : public ::testing::Test = std::make_shared(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); + Vector2d p1(1., 1.), p2(2., 1.2), p3(2.2, 2.), p4(3., 2.2), p5(3.2, 3.), p6(4., 3.2); - state.setValue(p1); + mStateSpace->expMap(p1, state); mNonStraightLine->addWaypoint(0., state); - state.setValue(p2); + mStateSpace->expMap(p2, state); mNonStraightLine->addWaypoint(1., state); - state.setValue(p3); + mStateSpace->expMap(p3, state); mNonStraightLine->addWaypoint(2., state); - state.setValue(p4); + mStateSpace->expMap(p4, state); mNonStraightLine->addWaypoint(3., state); - state.setValue(p5); + mStateSpace->expMap(p5, state); mNonStraightLine->addWaypoint(4., state); - state.setValue(p6); + mStateSpace->expMap(p6, state); mNonStraightLine->addWaypoint(5, state); double length = (p2 - p1).norm() + (p3 - p2).norm() + (p4 - p3).norm() @@ -94,15 +93,15 @@ class ParabolicSmootherTests : public ::testing::Test auto state = mStateSpace->createState(); Vector2d p1(1., 1.), p2(2., 1.2), p3(2.2, 2.), p4(3., 2.2), p5(3.2, 3.); - state.setValue(p1); + mStateSpace->expMap(p1, state); mNonStraightLineWithNonZeroStartTime->addWaypoint(1.2, state); - state.setValue(p2); + mStateSpace->expMap(p2, state); mNonStraightLineWithNonZeroStartTime->addWaypoint(2.3, state); - state.setValue(p3); + mStateSpace->expMap(p3, state); mNonStraightLineWithNonZeroStartTime->addWaypoint(3.4, state); - state.setValue(p4); + mStateSpace->expMap(p4, state); mNonStraightLineWithNonZeroStartTime->addWaypoint(4.5, state); - state.setValue(p5); + mStateSpace->expMap(p5, state); mNonStraightLineWithNonZeroStartTime->addWaypoint(5.5, state); double length = (p2 - p1).norm() + (p3 - p2).norm() + (p4 - p3).norm() @@ -127,8 +126,20 @@ class ParabolicSmootherTests : public ::testing::Test return length; } + void evaluate( + aikido::trajectory::Trajectory* traj, + double t, + Eigen::VectorXd& positions) + { + // This utility function is just for convenience; + // states should be reused when possible. + auto state = traj->getStateSpace()->createState(); + traj->evaluate(t, state); + traj->getStateSpace()->logMap(state, positions); + } + aikido::common::RNGWrapper mRng; - std::shared_ptr mStateSpace; + std::shared_ptr mStateSpace; Eigen::Vector2d mMaxVelocity; Eigen::Vector2d mMaxAcceleration; @@ -151,20 +162,15 @@ TEST_F(ParabolicSmootherTests, convertStraightInterpolatedToSpline) { auto spline = convertToSpline(*mStraightLine); - auto splineState = mStateSpace->createState(); - auto interpolatedState = mStateSpace->createState(); Eigen::VectorXd splineVec, interpolatedVec; - Eigen::VectorXd splineTangent, interpolatedTangent; const double stepSize = 1e-3; aikido::common::StepSequence seq( stepSize, true, true, spline->getStartTime(), spline->getEndTime()); for (double t : seq) { - spline->evaluate(t, splineState); - mStraightLine->evaluate(t, interpolatedState); - mStateSpace->logMap(splineState, splineVec); - mStateSpace->logMap(interpolatedState, interpolatedVec); + evaluate(spline.get(), t, splineVec); + evaluate(mStraightLine.get(), t, interpolatedVec); EXPECT_EIGEN_EQUAL(splineVec, interpolatedVec, mTolerance); } } @@ -173,44 +179,34 @@ TEST_F(ParabolicSmootherTests, convertNonStraightInterpolatedToSpline) { auto spline = convertToSpline(*mNonStraightLine); - auto splineState = mStateSpace->createState(); - auto interpolatedState = mStateSpace->createState(); Eigen::VectorXd splineVec, interpolatedVec; - Eigen::VectorXd splineTangent, interpolatedTangent; const double stepSize = 1e-3; aikido::common::StepSequence seq( stepSize, true, true, spline->getStartTime(), spline->getEndTime()); for (double t : seq) { - spline->evaluate(t, splineState); - mNonStraightLine->evaluate(t, interpolatedState); - mStateSpace->logMap(splineState, splineVec); - mStateSpace->logMap(interpolatedState, interpolatedVec); + evaluate(spline.get(), t, splineVec); + evaluate(mNonStraightLine.get(), t, interpolatedVec); EXPECT_EIGEN_EQUAL(splineVec, interpolatedVec, mTolerance); } } TEST_F( ParabolicSmootherTests, - convertNonStraightInterolatedWithNonZeroStartTimeToSpline) + convertNonStraightInterpolatedWithNonZeroStartTimeToSpline) { auto spline = convertToSpline(*mNonStraightLineWithNonZeroStartTime); - auto splineState = mStateSpace->createState(); - auto interpolatedState = mStateSpace->createState(); Eigen::VectorXd splineVec, interpolatedVec; - Eigen::VectorXd splineTangent, interpolatedTangent; const double stepSize = 1e-3; aikido::common::StepSequence seq( stepSize, true, true, spline->getStartTime(), spline->getEndTime()); for (double t : seq) { - spline->evaluate(t, splineState); - mNonStraightLineWithNonZeroStartTime->evaluate(t, interpolatedState); - mStateSpace->logMap(splineState, splineVec); - mStateSpace->logMap(interpolatedState, interpolatedVec); + evaluate(spline.get(), t, splineVec); + evaluate(mNonStraightLineWithNonZeroStartTime.get(), t, interpolatedVec); EXPECT_EIGEN_EQUAL(splineVec, interpolatedVec, mTolerance); } } @@ -232,16 +228,23 @@ TEST_F(ParabolicSmootherTests, doShortcut) mCheckResolution); // Position. - auto state = mStateSpace->createState(); - smoothedTrajectory->evaluate(smoothedTrajectory->getStartTime(), state); - auto startState = mStateSpace->createState(); - mNonStraightLine->evaluate(mNonStraightLine->getStartTime(), startState); - EXPECT_EIGEN_EQUAL(startState.getValue(), state.getValue(), mTolerance); - - smoothedTrajectory->evaluate(smoothedTrajectory->getEndTime(), state); - auto goalState = mStateSpace->createState(); - mNonStraightLine->evaluate(mNonStraightLine->getEndTime(), goalState); - EXPECT_EIGEN_EQUAL(goalState.getValue(), state.getValue(), mTolerance); + Eigen::VectorXd statePositions, startPositions, goalPositions; + + evaluate( + mNonStraightLine.get(), mNonStraightLine->getStartTime(), startPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getStartTime(), + statePositions); + EXPECT_EIGEN_EQUAL(startPositions, statePositions, mTolerance); + + evaluate( + mNonStraightLine.get(), mNonStraightLine->getEndTime(), goalPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getEndTime(), + statePositions); + EXPECT_EIGEN_EQUAL(goalPositions, statePositions, mTolerance); double shortenLength = getLength(smoothedTrajectory.get()); EXPECT_TRUE(shortenLength < mNonStraightLineLength); @@ -268,16 +271,22 @@ TEST_F(ParabolicSmootherTests, doBlend) mCheckResolution); // Position. - auto state = mStateSpace->createState(); - smoothedTrajectory->evaluate(smoothedTrajectory->getStartTime(), state); - auto startState = mStateSpace->createState(); - mNonStraightLine->evaluate(mNonStraightLine->getStartTime(), startState); - EXPECT_EIGEN_EQUAL(startState.getValue(), state.getValue(), mTolerance); - - smoothedTrajectory->evaluate(smoothedTrajectory->getEndTime(), state); - auto goalState = mStateSpace->createState(); - mNonStraightLine->evaluate(mNonStraightLine->getEndTime(), goalState); - EXPECT_EIGEN_EQUAL(goalState.getValue(), state.getValue(), mTolerance); + Eigen::VectorXd statePositions, startPositions, goalPositions; + evaluate( + mNonStraightLine.get(), mNonStraightLine->getStartTime(), startPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getStartTime(), + statePositions); + EXPECT_EIGEN_EQUAL(startPositions, statePositions, mTolerance); + + evaluate( + mNonStraightLine.get(), mNonStraightLine->getEndTime(), goalPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getEndTime(), + statePositions); + EXPECT_EIGEN_EQUAL(goalPositions, statePositions, mTolerance); double shortenLength = getLength(smoothedTrajectory.get()); EXPECT_TRUE(shortenLength < mNonStraightLineLength); @@ -306,16 +315,22 @@ TEST_F(ParabolicSmootherTests, doShortcutAndBlend) mCheckResolution); // Position. - auto state = mStateSpace->createState(); - smoothedTrajectory->evaluate(smoothedTrajectory->getStartTime(), state); - auto startState = mStateSpace->createState(); - mNonStraightLine->evaluate(mNonStraightLine->getStartTime(), startState); - EXPECT_EIGEN_EQUAL(startState.getValue(), state.getValue(), mTolerance); - - smoothedTrajectory->evaluate(smoothedTrajectory->getEndTime(), state); - auto goalState = mStateSpace->createState(); - mNonStraightLine->evaluate(mNonStraightLine->getEndTime(), goalState); - EXPECT_EIGEN_EQUAL(goalState.getValue(), state.getValue(), mTolerance); + Eigen::VectorXd statePositions, startPositions, goalPositions; + evaluate( + mNonStraightLine.get(), mNonStraightLine->getStartTime(), startPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getStartTime(), + statePositions); + EXPECT_EIGEN_EQUAL(startPositions, statePositions, mTolerance); + + evaluate( + mNonStraightLine.get(), mNonStraightLine->getEndTime(), goalPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getEndTime(), + statePositions); + EXPECT_EIGEN_EQUAL(goalPositions, statePositions, mTolerance); double shortenLength = getLength(smoothedTrajectory.get()); EXPECT_TRUE(shortenLength < mNonStraightLineLength); diff --git a/tests/planner/parabolic/test_ParabolicTimer.cpp b/tests/planner/parabolic/test_ParabolicTimer.cpp index 8acb60634d..1c827fc085 100644 --- a/tests/planner/parabolic/test_ParabolicTimer.cpp +++ b/tests/planner/parabolic/test_ParabolicTimer.cpp @@ -11,7 +11,7 @@ using Eigen::Vector2d; using Eigen::Vector3d; using aikido::trajectory::Interpolated; using aikido::statespace::GeodesicInterpolator; -using aikido::statespace::R2; +using aikido::statespace::R1; using aikido::statespace::CartesianProduct; using aikido::statespace::SO2; using aikido::statespace::SO3; @@ -27,7 +27,10 @@ class ParabolicTimerTests : public ::testing::Test protected: void SetUp() override { - mStateSpace = std::make_shared(); + std::vector subspaces; + for (std::size_t i = 0; i < 2; ++i) + subspaces.emplace_back(std::make_shared()); + mStateSpace = std::make_shared(subspaces); mMaxVelocity = Eigen::Vector2d(1., 1.); mMaxAcceleration = Eigen::Vector2d(2., 2.); @@ -36,14 +39,15 @@ class ParabolicTimerTests : public ::testing::Test auto state = mStateSpace->createState(); - state.setValue(Vector2d(1., 2.)); + Vector2d p1(1, 2), p2(3, 4); + mStateSpace->expMap(p1, state); mStraightLine->addWaypoint(0., state); - state.setValue(Vector2d(3., 4.)); + mStateSpace->expMap(p2, state); mStraightLine->addWaypoint(1., state); } - std::shared_ptr mStateSpace; + std::shared_ptr mStateSpace; Eigen::Vector2d mMaxVelocity; Eigen::Vector2d mMaxAcceleration; @@ -107,27 +111,31 @@ TEST_F(ParabolicTimerTests, StartsAtNonZeroTime) Interpolated inputTrajectory(mStateSpace, mInterpolator); auto state = mStateSpace->createState(); - Eigen::VectorXd tangentVector; + Eigen::VectorXd positions; + Vector2d p1(1, 2), p2(2, 3); // This is the same test as StraightLine_TriangularProfile, except that the // trajectory starts at a non-zero time. - state.setValue(Vector2d(1., 2.)); + mStateSpace->expMap(p1, state); inputTrajectory.addWaypoint(1., state); - state.setValue(Vector2d(2., 3.)); + mStateSpace->expMap(p2, state); inputTrajectory.addWaypoint(3., state); auto timedTrajectory = computeParabolicTiming( inputTrajectory, Vector2d::Constant(2.), Vector2d::Constant(1.)); timedTrajectory->evaluate(1., state); - EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(positions)); timedTrajectory->evaluate(2., state); - EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(positions)); timedTrajectory->evaluate(3., state); - EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(positions)); } TEST_F(ParabolicTimerTests, InterploatedSplineEquivalence) @@ -136,13 +144,16 @@ TEST_F(ParabolicTimerTests, InterploatedSplineEquivalence) auto state = mStateSpace->createState(); auto state2 = mStateSpace->createState(); + Eigen::VectorXd positions, positions2; // This is the same test as StraightLine_TriangularProfile, except that the // trajectory starts at a non-zero time. - state.setValue(Vector2d(1., 2.)); + Vector2d p1(1, 2), p2(2, 3); + + mStateSpace->expMap(p1, state); interpolated.addWaypoint(1., state); - state.setValue(Vector2d(2., 3.)); + mStateSpace->expMap(p2, state); interpolated.addWaypoint(3., state); auto spline = convertToSpline(interpolated); @@ -154,15 +165,21 @@ TEST_F(ParabolicTimerTests, InterploatedSplineEquivalence) timedInterpolated->evaluate(1., state); timedSpline->evaluate(1., state2); - EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_TRUE(positions2.isApprox(positions)); timedInterpolated->evaluate(2., state); timedSpline->evaluate(2., state2); - EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_TRUE(positions2.isApprox(positions)); timedInterpolated->evaluate(3., state); timedSpline->evaluate(3., state2); - EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_TRUE(positions2.isApprox(positions)); } TEST_F(ParabolicTimerTests, StraightLine_TriangularProfile) @@ -171,14 +188,15 @@ TEST_F(ParabolicTimerTests, StraightLine_TriangularProfile) auto state = mStateSpace->createState(); Eigen::VectorXd tangentVector; + Vector2d p1(1, 2), p2(2, 3); // The optimal timing of this trajectory should be a triangle centered at t = // 1that accelerates at 1 rad/s^2 for 1 s, then deaccelerates at -1 rad/s^2 // for 1 s. This corresponds to moving each axis through 2 rad. - state.setValue(Vector2d(1., 2.)); + mStateSpace->expMap(p1, state); inputTrajectory.addWaypoint(0., state); - state.setValue(Vector2d(2., 3.)); + mStateSpace->expMap(p2, state); inputTrajectory.addWaypoint(2., state); auto timedTrajectory = computeParabolicTiming( @@ -189,15 +207,20 @@ TEST_F(ParabolicTimerTests, StraightLine_TriangularProfile) EXPECT_EQ(2, timedTrajectory->getNumSegments()); EXPECT_DOUBLE_EQ(2., timedTrajectory->getDuration()); + Eigen::VectorXd positions; + // Position. timedTrajectory->evaluate(0., state); - EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(positions)); timedTrajectory->evaluate(1., state); - EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(positions)); timedTrajectory->evaluate(2., state); - EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(positions)); // Velocity timedTrajectory->evaluateDerivative(0.5, 1, tangentVector); @@ -223,15 +246,16 @@ TEST_F(ParabolicTimerTests, StraightLine_TrapezoidalProfile) auto state = mStateSpace->createState(); Eigen::VectorXd tangentVector; + Vector2d p1(1, 2), p2(3, 4); // The optimal timing of this trajectory should be a trapezoid that: // - accelerates at 1 rad/s^2 for 1 s // - coasts at 1 m/s for 1 s // - deaccelerates at -1 rad/s^2 for 1 s - state.setValue(Vector2d(1., 2.)); + mStateSpace->expMap(p1, state); inputTrajectory.addWaypoint(0., state); - state.setValue(Vector2d(3., 4.)); + mStateSpace->expMap(p2, state); inputTrajectory.addWaypoint(2., state); auto timedTrajectory = computeParabolicTiming( @@ -242,18 +266,24 @@ TEST_F(ParabolicTimerTests, StraightLine_TrapezoidalProfile) EXPECT_EQ(3, timedTrajectory->getNumSegments()); EXPECT_DOUBLE_EQ(3., timedTrajectory->getDuration()); + Eigen::VectorXd positions; + // Position. timedTrajectory->evaluate(0., state); - EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(positions)); timedTrajectory->evaluate(1., state); - EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(positions)); timedTrajectory->evaluate(2., state); - EXPECT_TRUE(Vector2d(2.5, 3.5).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(2.5, 3.5).isApprox(positions)); timedTrajectory->evaluate(3., state); - EXPECT_TRUE(Vector2d(3.0, 4.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(3.0, 4.0).isApprox(positions)); // Velocity timedTrajectory->evaluateDerivative(0.5, 1, tangentVector); @@ -293,6 +323,7 @@ TEST_F(ParabolicTimerTests, StraightLine_DifferentAccelerationLimits) auto state = mStateSpace->createState(); Eigen::VectorXd tangentVector; + Vector2d p1(1, 2), p2(3, 4); // The optimal timing of this trajectory should be a trapezoid that: // - accelerates at 1 rad/s^2 for 1 s @@ -302,10 +333,11 @@ TEST_F(ParabolicTimerTests, StraightLine_DifferentAccelerationLimits) // Note that the second dimension of the state space could result in a faster // timing by executing a triangular velocity profile. This is not possible // because the first dimension has a lower acceleration limit. - state.setValue(Vector2d(1., 2.)); + + mStateSpace->expMap(p1, state); inputTrajectory.addWaypoint(0., state); - state.setValue(Vector2d(3., 4.)); + mStateSpace->expMap(p2, state); inputTrajectory.addWaypoint(2., state); auto timedTrajectory = computeParabolicTiming( @@ -336,6 +368,13 @@ TEST_F(ParabolicTimerTests, UnsupportedStateSpace_Throws) EXPECT_THROW({ convertToSpline(inputTrajectory); }, std::invalid_argument); } +TEST_F(ParabolicTimerTests, SupportedCartesianProduct_DoesNotThrow) +{ + EXPECT_NO_THROW({ + computeParabolicTiming(*mStraightLine, mMaxVelocity, mMaxAcceleration); + }); +} + TEST_F(ParabolicTimerTests, UnsupportedCartesianProduct_Throws) { auto stateSpace = std::make_shared( @@ -357,32 +396,6 @@ TEST_F(ParabolicTimerTests, UnsupportedCartesianProduct_Throws) EXPECT_THROW({ convertToSpline(inputTrajectory); }, std::invalid_argument); } -TEST_F(ParabolicTimerTests, SupportedCartesianProduct_DoesNotThrow) -{ - auto stateSpace = std::make_shared( - std::vector{ - std::make_shared(), std::make_shared(), - }); - auto state = stateSpace->createState(); - - std::shared_ptr interpolator - = std::make_shared(stateSpace); - - Interpolated inputTrajectory(stateSpace, interpolator); - - state.getSubStateHandle(0).setValue(Vector2d::Zero()); - state.getSubStateHandle(1).fromAngle(0.); - inputTrajectory.addWaypoint(0., state); - - state.getSubStateHandle(0).setValue(Vector2d::Zero()); - state.getSubStateHandle(1).fromAngle(M_PI_2); - inputTrajectory.addWaypoint(1., state); - - EXPECT_NO_THROW({ - computeParabolicTiming(inputTrajectory, Vector3d::Ones(), Vector3d::Ones()); - }); -} - // TODO: Test what happens when two waypoints are coincident. // TODO: Add a test for different velocity limits. // TODO: Add a test where DOFs have different ramp transition points. diff --git a/tests/planner/parabolic/test_SmoothPostProcessor.cpp b/tests/planner/parabolic/test_SmoothPostProcessor.cpp index a375a51856..98ef489138 100644 --- a/tests/planner/parabolic/test_SmoothPostProcessor.cpp +++ b/tests/planner/parabolic/test_SmoothPostProcessor.cpp @@ -2,18 +2,18 @@ #include #include #include +#include #include #include #include "eigen_tests.hpp" using Eigen::Vector2d; -using Eigen::Vector3d; using aikido::trajectory::Interpolated; +using aikido::statespace::ConstStateSpacePtr; using aikido::statespace::GeodesicInterpolator; -using aikido::statespace::R2; -using aikido::statespace::StateSpacePtr; +using aikido::statespace::R1; +using aikido::statespace::CartesianProduct; using aikido::constraint::Satisfied; -using aikido::common::cloneRNGFrom; using aikido::planner::parabolic::ParabolicSmoother; class SmoothPostProcessorTests : public ::testing::Test @@ -24,7 +24,10 @@ class SmoothPostProcessorTests : public ::testing::Test void SetUp() override { mRng = aikido::common::RNGWrapper(0); - mStateSpace = std::make_shared(); + std::vector subspaces; + for (std::size_t i = 0; i < 2; ++i) + subspaces.emplace_back(std::make_shared()); + mStateSpace = std::make_shared(subspaces); mMaxVelocity = Eigen::Vector2d(20., 20.); mMaxAcceleration = Eigen::Vector2d(10., 10.); @@ -39,17 +42,17 @@ class SmoothPostProcessorTests : public ::testing::Test auto state = mStateSpace->createState(); Vector2d p1(1., 1.), p2(2., 1.2), p3(2.2, 2.), p4(3., 2.2), p5(3.2, 3.), p6(4., 3.2); - state.setValue(p1); + mStateSpace->expMap(p1, state); mTrajectory->addWaypoint(0., state); - state.setValue(p2); + mStateSpace->expMap(p2, state); mTrajectory->addWaypoint(1., state); - state.setValue(p3); + mStateSpace->expMap(p3, state); mTrajectory->addWaypoint(2., state); - state.setValue(p4); + mStateSpace->expMap(p4, state); mTrajectory->addWaypoint(3., state); - state.setValue(p5); + mStateSpace->expMap(p5, state); mTrajectory->addWaypoint(4., state); - state.setValue(p6); + mStateSpace->expMap(p6, state); mTrajectory->addWaypoint(5, state); double length = (p2 - p1).norm() + (p3 - p2).norm() + (p4 - p3).norm() @@ -74,8 +77,20 @@ class SmoothPostProcessorTests : public ::testing::Test return length; } + void evaluate( + aikido::trajectory::Trajectory* traj, + double t, + Eigen::VectorXd& positions) + { + // This utility function is just for convenience; + // states should be reused when possible. + auto state = traj->getStateSpace()->createState(); + traj->evaluate(t, state); + traj->getStateSpace()->logMap(state, positions); + } + aikido::common::RNGWrapper mRng; - std::shared_ptr mStateSpace; + std::shared_ptr mStateSpace; Eigen::Vector2d mMaxVelocity; Eigen::Vector2d mMaxAcceleration; @@ -113,16 +128,20 @@ TEST_F(SmoothPostProcessorTests, useShortcutting) = testSmoothPostProcessor.postprocess(*mTrajectory, mRng, testable); // Position. - auto state = mStateSpace->createState(); - smoothedTrajectory->evaluate(smoothedTrajectory->getStartTime(), state); - auto startState = mStateSpace->createState(); - mTrajectory->evaluate(mTrajectory->getStartTime(), startState); - EXPECT_EIGEN_EQUAL(startState.getValue(), state.getValue(), mTolerance); - - smoothedTrajectory->evaluate(smoothedTrajectory->getEndTime(), state); - auto goalState = mStateSpace->createState(); - mTrajectory->evaluate(mTrajectory->getEndTime(), goalState); - EXPECT_EIGEN_EQUAL(goalState.getValue(), state.getValue(), mTolerance); + Eigen::VectorXd statePositions, startPositions, goalPositions; + evaluate(mTrajectory.get(), mTrajectory->getStartTime(), startPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getStartTime(), + statePositions); + EXPECT_EIGEN_EQUAL(startPositions, statePositions, mTolerance); + + evaluate(mTrajectory.get(), mTrajectory->getEndTime(), goalPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getEndTime(), + statePositions); + EXPECT_EIGEN_EQUAL(goalPositions, statePositions, mTolerance); double shortenLength = getLength(smoothedTrajectory.get()); EXPECT_TRUE(shortenLength < mOriginalTrajectoryLength); @@ -154,16 +173,20 @@ TEST_F(SmoothPostProcessorTests, useBlend) = testSmoothPostProcessor.postprocess(*mTrajectory, mRng, testable); // Position. - auto state = mStateSpace->createState(); - smoothedTrajectory->evaluate(smoothedTrajectory->getStartTime(), state); - auto startState = mStateSpace->createState(); - mTrajectory->evaluate(mTrajectory->getStartTime(), startState); - EXPECT_EIGEN_EQUAL(startState.getValue(), state.getValue(), mTolerance); - - smoothedTrajectory->evaluate(smoothedTrajectory->getEndTime(), state); - auto goalState = mStateSpace->createState(); - mTrajectory->evaluate(mTrajectory->getEndTime(), goalState); - EXPECT_EIGEN_EQUAL(goalState.getValue(), state.getValue(), mTolerance); + Eigen::VectorXd statePositions, startPositions, goalPositions; + evaluate(mTrajectory.get(), mTrajectory->getStartTime(), startPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getStartTime(), + statePositions); + EXPECT_EIGEN_EQUAL(startPositions, statePositions, mTolerance); + + evaluate(mTrajectory.get(), mTrajectory->getEndTime(), goalPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getEndTime(), + statePositions); + EXPECT_EIGEN_EQUAL(goalPositions, statePositions, mTolerance); double shortenLength = getLength(smoothedTrajectory.get()); EXPECT_TRUE(shortenLength < mOriginalTrajectoryLength); @@ -195,16 +218,20 @@ TEST_F(SmoothPostProcessorTests, useShortcuttingAndBlend) = testSmoothPostProcessor.postprocess(*mTrajectory, mRng, testable); // Position. - auto state = mStateSpace->createState(); - smoothedTrajectory->evaluate(smoothedTrajectory->getStartTime(), state); - auto startState = mStateSpace->createState(); - mTrajectory->evaluate(mTrajectory->getStartTime(), startState); - EXPECT_EIGEN_EQUAL(startState.getValue(), state.getValue(), mTolerance); - - smoothedTrajectory->evaluate(smoothedTrajectory->getEndTime(), state); - auto goalState = mStateSpace->createState(); - mTrajectory->evaluate(mTrajectory->getEndTime(), goalState); - EXPECT_EIGEN_EQUAL(goalState.getValue(), state.getValue(), mTolerance); + Eigen::VectorXd statePositions, startPositions, goalPositions; + evaluate(mTrajectory.get(), mTrajectory->getStartTime(), startPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getStartTime(), + statePositions); + EXPECT_EIGEN_EQUAL(startPositions, statePositions, mTolerance); + + evaluate(mTrajectory.get(), mTrajectory->getEndTime(), goalPositions); + evaluate( + smoothedTrajectory.get(), + smoothedTrajectory->getEndTime(), + statePositions); + EXPECT_EIGEN_EQUAL(goalPositions, statePositions, mTolerance); double shortenLength = getLength(smoothedTrajectory.get()); EXPECT_TRUE(shortenLength < mOriginalTrajectoryLength); diff --git a/tests/planner/parabolic/test_TimePostProcessor.cpp b/tests/planner/parabolic/test_TimePostProcessor.cpp index 7c4d58afc7..2df27fe363 100644 --- a/tests/planner/parabolic/test_TimePostProcessor.cpp +++ b/tests/planner/parabolic/test_TimePostProcessor.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -8,8 +9,10 @@ using Eigen::Vector2d; using aikido::trajectory::Interpolated; +using aikido::statespace::CartesianProduct; +using aikido::statespace::ConstStateSpacePtr; using aikido::statespace::GeodesicInterpolator; -using aikido::statespace::R2; +using aikido::statespace::R1; using aikido::trajectory::convertToSpline; using aikido::planner::parabolic::ParabolicTimer; @@ -19,12 +22,15 @@ class TimePostProcessorTests : public ::testing::Test void SetUp() override { mRng = aikido::common::RNGWrapper(0); - mStateSpace = std::make_shared(); + std::vector subspaces; + for (std::size_t i = 0; i < 2; ++i) + subspaces.emplace_back(std::make_shared()); + mStateSpace = std::make_shared(subspaces); mInterpolator = std::make_shared(mStateSpace); } aikido::common::RNGWrapper mRng; - std::shared_ptr mStateSpace; + std::shared_ptr mStateSpace; std::shared_ptr mInterpolator; }; @@ -37,14 +43,15 @@ TEST_F(TimePostProcessorTests, testTime) auto state = mStateSpace->createState(); Eigen::VectorXd tangentVector; + Vector2d p1(1, 2), p2(2, 3); // The optimal timing of this trajectory should be a triangle centered at t = // 1that accelerates at 1 rad/s^2 for 1 s, then deaccelerates at -1 rad/s^2 // for 1 s. This corresponds to moving each axis through 2 rad. - state.setValue(Vector2d(1., 2.)); + mStateSpace->expMap(p1, state); inputTrajectory.addWaypoint(0., state); - state.setValue(Vector2d(2., 3.)); + mStateSpace->expMap(p2, state); inputTrajectory.addWaypoint(2., state); auto timedTrajectory @@ -55,15 +62,20 @@ TEST_F(TimePostProcessorTests, testTime) EXPECT_EQ(2, timedTrajectory->getNumSegments()); EXPECT_DOUBLE_EQ(2., timedTrajectory->getDuration()); + Eigen::VectorXd positions; + // Position. timedTrajectory->evaluate(0., state); - EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(positions)); timedTrajectory->evaluate(1., state); - EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(positions)); timedTrajectory->evaluate(2., state); - EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(positions)); // Velocity timedTrajectory->evaluateDerivative(0.5, 1, tangentVector); @@ -92,13 +104,15 @@ TEST_F(TimePostProcessorTests, testSplineTiming) auto state = mStateSpace->createState(); auto state2 = mStateSpace->createState(); + Eigen::VectorXd positions, positions2; + Vector2d p1(1, 2), p2(2, 3); // This is the same test as StraightLine_TriangularProfile, except that the // trajectory starts at a non-zero time. - state.setValue(Vector2d(1., 2.)); + mStateSpace->expMap(p1, state); interpolated.addWaypoint(1., state); - state.setValue(Vector2d(2., 3.)); + mStateSpace->expMap(p2, state); interpolated.addWaypoint(3., state); auto spline = convertToSpline(interpolated); @@ -109,13 +123,19 @@ TEST_F(TimePostProcessorTests, testSplineTiming) timedInterpolated->evaluate(1., state); timedSpline->evaluate(1., state2); - EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_TRUE(positions2.isApprox(positions)); timedInterpolated->evaluate(2., state); timedSpline->evaluate(2., state2); - EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_TRUE(positions2.isApprox(positions)); timedInterpolated->evaluate(3., state); timedSpline->evaluate(3., state2); - EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + mStateSpace->logMap(state2, positions2); + EXPECT_TRUE(positions2.isApprox(positions)); } diff --git a/tests/trajectory/CMakeLists.txt b/tests/trajectory/CMakeLists.txt index 2d4b3bde66..f03bb20c6f 100644 --- a/tests/trajectory/CMakeLists.txt +++ b/tests/trajectory/CMakeLists.txt @@ -12,3 +12,8 @@ aikido_add_test(test_BSpline test_BSpline.cpp) target_link_libraries(test_BSpline "${PROJECT_NAME}_trajectory" "${PROJECT_NAME}_statespace") + +aikido_add_test(test_util test_util.cpp) +target_link_libraries(test_util + "${PROJECT_NAME}_trajectory" + "${PROJECT_NAME}_statespace") diff --git a/tests/trajectory/test_SplineTrajectory.cpp b/tests/trajectory/test_SplineTrajectory.cpp index d8154972ba..28ad883659 100644 --- a/tests/trajectory/test_SplineTrajectory.cpp +++ b/tests/trajectory/test_SplineTrajectory.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -15,10 +16,14 @@ class SplineTest : public ::testing::Test protected: void SetUp() override { - mStateSpace = std::make_shared(); - - mStartState = static_cast(mStateSpace->allocateState()); - mStateSpace->setValue(mStartState, START_VALUE); + std::vector subspaces; + for (std::size_t i = 0; i < 2; ++i) + subspaces.emplace_back(std::make_shared()); + mStateSpace = std::make_shared(subspaces); + + mStartState + = static_cast(mStateSpace->allocateState()); + mStateSpace->expMap(START_VALUE, mStartState); } void TearDown() override @@ -26,8 +31,8 @@ class SplineTest : public ::testing::Test mStateSpace->freeState(mStartState); } - std::shared_ptr mStateSpace; - R2::State* mStartState; + std::shared_ptr mStateSpace; + CartesianProduct::State* mStartState; std::shared_ptr mTrajectory; }; @@ -220,7 +225,9 @@ TEST_F(SplineTest, evaluate_EvaluateStart_ReturnsStart) auto state = mStateSpace->createState(); trajectory.evaluate(3., state); - EXPECT_TRUE(START_VALUE.isApprox(state.getValue())); + Eigen::VectorXd positions; + mStateSpace->logMap(state, positions); + EXPECT_TRUE(START_VALUE.isApprox(positions)); } TEST_F(SplineTest, evaluate_EvaluateEnd_ReturnsEnd) @@ -232,13 +239,17 @@ TEST_F(SplineTest, evaluate_EvaluateEnd_ReturnsEnd) Spline trajectory(mStateSpace, 3.); auto state = mStateSpace->createState(); + Eigen::VectorXd positions; + trajectory.addSegment(coefficients1, 1., mStartState); trajectory.evaluate(4., state); - EXPECT_TRUE(Vector2d(2., 4.).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(2., 4.).isApprox(positions)); trajectory.addSegment(coefficients2, 2.); trajectory.evaluate(6., state); - EXPECT_TRUE(Vector2d(8., 12.).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(8., 12.).isApprox(positions)); } TEST_F(SplineTest, evaluate_Middle_ReturnsInterpolation) @@ -255,23 +266,31 @@ TEST_F(SplineTest, evaluate_Middle_ReturnsInterpolation) auto state = mStateSpace->createState(); + Eigen::VectorXd positions; + trajectory.evaluate(3.5, state); - EXPECT_TRUE(Vector2d(1.25, 2.75).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1.25, 2.75).isApprox(positions)); trajectory.evaluate(4.0, state); - EXPECT_TRUE(Vector2d(2.00, 4.00).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(2.00, 4.00).isApprox(positions)); trajectory.evaluate(5.0, state); - EXPECT_TRUE(Vector2d(5.00, 8.00).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(5.00, 8.00).isApprox(positions)); trajectory.evaluate(6.0, state); - EXPECT_TRUE(Vector2d(12., 16.).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(12., 16.).isApprox(positions)); trajectory.evaluate(7.5, state); - EXPECT_TRUE(Vector2d(21.75, 27.25).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(21.75, 27.25).isApprox(positions)); trajectory.evaluate(9.0, state); - EXPECT_TRUE(Vector2d(45.00, 52.00).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(45.00, 52.00).isApprox(positions)); } TEST_F(SplineTest, evaluateDerivative_IsEmpty_Throws) @@ -363,7 +382,7 @@ TEST_F(SplineTest, FindTimeOfClosetStateOnTrajectory) std::shared_ptr trajectory = std::make_shared(mStateSpace); auto startState = mStateSpace->createState(); - mStateSpace->setValue(startState, Vector2d(1., 1.)); + mStateSpace->expMap(Vector2d(1., 1.), startState); trajectory->addSegment(coefficients, 2., startState); Vector2d query(1., 2.); @@ -382,21 +401,26 @@ TEST_F(SplineTest, ConcatenateTwoTrajectories) auto startState1 = mStateSpace->createState(); auto startState2 = mStateSpace->createState(); auto state = mStateSpace->createState(); - mStateSpace->setValue(startState1, Vector2d(1., 2.)); - mStateSpace->setValue(startState2, Vector2d(3., 6.)); + mStateSpace->expMap(Vector2d(1., 2.), startState1); + mStateSpace->expMap(Vector2d(3., 6.), startState2); traj1.addSegment(coefficients1, 1.0, startState1); traj2.addSegment(coefficients2, 1.0, startState2); auto newTraj = aikido::trajectory::concatenate(traj1, traj2); + Eigen::VectorXd positions; + newTraj->evaluate(5.0, state); - EXPECT_TRUE(Vector2d(1., 2.).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(1., 2.).isApprox(positions)); newTraj->evaluate(6.0, state); - EXPECT_TRUE(Vector2d(3., 6.).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(3., 6.).isApprox(positions)); newTraj->evaluate(7.0, state); - EXPECT_TRUE(Vector2d(6., 10.).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(6., 10.).isApprox(positions)); EXPECT_DOUBLE_EQ( traj1.getDuration() + traj2.getDuration(), newTraj->getDuration()); @@ -409,17 +433,21 @@ TEST_F(SplineTest, CreatePartialTrajectory) Spline trajectory(mStateSpace); auto startState = mStateSpace->createState(); - mStateSpace->setValue(startState, Vector2d(1., 1.)); + mStateSpace->expMap(Vector2d(1., 1.), startState); trajectory.addSegment(coefficients, 2., startState); auto state = mStateSpace->createState(); auto partialTraj = createPartialTrajectory(trajectory, 1.0); + Eigen::VectorXd positions; + partialTraj->evaluate(0., state); - EXPECT_TRUE(Vector2d(2., 2.).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(2., 2.).isApprox(positions)); partialTraj->evaluate(1., state); - EXPECT_TRUE(Vector2d(3., 3.).isApprox(state.getValue())); + mStateSpace->logMap(state, positions); + EXPECT_TRUE(Vector2d(3., 3.).isApprox(positions)); EXPECT_EQ(partialTraj->getDuration() + 1.0, trajectory.getDuration()); } diff --git a/tests/trajectory/test_util.cpp b/tests/trajectory/test_util.cpp new file mode 100644 index 0000000000..0e7e3ca45a --- /dev/null +++ b/tests/trajectory/test_util.cpp @@ -0,0 +1,104 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using std::shared_ptr; +using std::make_shared; +using aikido::trajectory::Interpolated; +using aikido::statespace::R1; +using aikido::statespace::CartesianProduct; +using aikido::trajectory::toR1JointTrajectory; +using aikido::statespace::ConstStateSpacePtr; + +//============================================================================== +class TrajectoryConversionTest : 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; + + TrajectoryConversionTest() + : skel{dart::dynamics::Skeleton::create("skel")} + , jn_bn{skel->createJointAndBodyNodePair()} + , stateSpace{make_shared(skel.get())} + , interpolator(make_shared(stateSpace)) + { + // Do nothing + } + + // DART setup + SkeletonPtr skel; + std::pair jn_bn; + + // Arguments for planner + shared_ptr stateSpace; + shared_ptr interpolator; +}; + +//============================================================================== +TEST_F(TrajectoryConversionTest, SuccessfulConversionToR1) +{ + // Create Trajectory. + auto trajectory = std::make_shared(stateSpace, interpolator); + + // Add waypoints + Eigen::VectorXd stateVector(stateSpace->getDimension()); + + stateVector << 3.0; + auto s1 = stateSpace->createState(); + stateSpace->convertPositionsToState(stateVector, s1); + trajectory->addWaypoint(0, s1); + + stateVector << M_PI; + auto s2 = stateSpace->createState(); + stateSpace->convertPositionsToState(stateVector, s2); + trajectory->addWaypoint(1, s2); + + stateVector << -3.0; + auto s3 = stateSpace->createState(); + stateSpace->convertPositionsToState(stateVector, s3); + trajectory->addWaypoint(2, s3); + + // Convert the trajectory. + auto convertedTrajectory = toR1JointTrajectory(*(trajectory.get())); + + // // Test the states in the interpolated trajectory. + std::vector subspaces; + for (std::size_t i = 0; i < stateSpace->getDimension(); ++i) + subspaces.emplace_back(std::make_shared()); + + auto rSpace = std::make_shared(subspaces); + auto rInterpolator = std::make_shared(rSpace); + auto rTrajectory = std::make_shared(rSpace, rInterpolator); + + EXPECT_EQ(3, convertedTrajectory->getNumWaypoints()); + + Eigen::VectorXd testVector(convertedTrajectory->getNumWaypoints()); + testVector << 3.0, M_PI, 2 * M_PI - 3.0; + for (std::size_t i = 0; i < convertedTrajectory->getNumWaypoints(); ++i) + { + auto sstate = convertedTrajectory->getWaypoint(i); + stateSpace->logMap(sstate, stateVector); + EXPECT_EQ(stateVector(0), testVector(i)); + } + auto sstate = stateSpace->createState(); + convertedTrajectory->evaluate( + convertedTrajectory->getDuration() * 0.75, sstate); + stateSpace->logMap(sstate, stateVector); + EXPECT_EQ(stateVector(0), (testVector(2) + testVector(1)) / 2.0); +}