Remove incorrect spline-conversions

CHANGELOG.md

@@ -49,7 +49,8 @@
 
   * Added B-spline trajectory: [#453](https://github.com/personalrobotics/aikido/pull/453)
   * Added trajectory utility functions: [#462](https://github.com/personalrobotics/aikido/pull/462)
-  * Fixes toR1JointTrajectory to copy Waypoints with their time information: [#508](https://github.com/personalrobotics/aikido/pull/510)
+  * Fixed toR1JointTrajectory to copy Waypoints with their time information: [#510](https://github.com/personalrobotics/aikido/pull/510)
+  * Removed incorrect Spline to Interpolated conversions: [#511](https://github.com/personalrobotics/aikido/pull/511)
 
 * Planner
 

include/aikido/planner/kunzretimer/KunzRetimer.hpp

@@ -38,34 +38,6 @@ std::unique_ptr<aikido::trajectory::Spline> computeKunzTiming(
     double maxDeviation = 1e-2,
     double timeStep = 0.1);
 
-/// Computes the time-optimal timing of a spline trajectory under velocity
-/// and acceleration bounds. The output is another parabolic spline, encoded
-/// in cubic polynomials. In retiming, the input trajectory is used as a
-/// piecewise Geodesic trajectory (only the geometry of the input trajectory
-/// is considered; and the velocity information is ignored. It firstly
-/// preprocesses a non-differentiable path to a differentiable one by adding
-/// circular blends; and then \b exactly follows the preprocessed path.
-///
-/// The output trajectory consists of a sequence of trapezoidal velocity
-/// profiles that implement bang-bang control. This function curently only
-/// supports \c RealVector, \c SO2, and compound state spaces of those types.
-/// Additionally, this function requires that \c inputTrajectory to be
-/// a spline (only the geometry of the trajectory is considered in retiming).
-///
-/// \param[in] inputTrajectory Input piecewise Geodesic trajectory
-/// \param[in] maxVelocity Maximum velocity for each dimension
-/// \param[in] maxAcceleration Maximum acceleration for each dimension
-/// \param[in] maxDeviation Maximum deviation from a waypoint in doing circular
-/// blending around the waypoint
-/// \param[in] timeStep Time step in following the path
-/// \return Time optimal trajectory that satisfies acceleration constraints
-std::unique_ptr<aikido::trajectory::Spline> computeKunzTiming(
-    const aikido::trajectory::Spline& inputTrajectory,
-    const Eigen::VectorXd& maxVelocity,
-    const Eigen::VectorXd& maxAcceleration,
-    double maxDeviation = 1e-2,
-    double timeStep = 0.1);
-
 /// Class for performing time-optimal trajectory retiming following subject to
 /// velocity and acceleration limits.
 class KunzRetimer : public aikido::planner::TrajectoryPostProcessor

include/aikido/trajectory/util.hpp

@@ -17,16 +17,6 @@ namespace trajectory {
 /// \return A spline trajectory
 UniqueSplinePtr convertToSpline(const Interpolated& inputTrajectory);
 
-/// Converts a piecewise linear spline trajectory to an interpolated trajectory
-/// using a given interpolator.
-///
-/// \param[in] traj Spline trajectory
-/// \param[in] interpolator Interpolator used in connecting two ends of a
-/// segment
-/// \return An interpolated trajectory
-UniqueInterpolatedPtr convertToInterpolated(
-    const Spline& traj, statespace::ConstInterpolatorPtr interpolator);
-
 /// Concatenates two interpolated trajectories.
 ///
 /// This function concatenates two interpolated trajectories into one
@@ -45,17 +35,6 @@ UniqueInterpolatedPtr convertToInterpolated(
 UniqueInterpolatedPtr concatenate(
     const Interpolated& traj1, const Interpolated& traj2);
 
-/// Concatenates two spline trajectories.
-///
-/// This function converts two spline trajectories into geodesically
-/// interpolated for concatenation, then converts the concatenated interpolated
-/// to spline. The state spaces of two trajectories should be the same.
-///
-/// \param[in] traj1 The first half spline trajectory
-/// \param[in] traj2 The second half spline trajectory
-/// \return The concatenated spline trajectory
-UniqueSplinePtr concatenate(const Spline& traj1, const Spline& traj2);
-
 /// Finds the time of the closest state on a trajectory to a given state.
 ///
 /// This function checks discrete states on [t0, t1, ..., tn] where t0 and tn
@@ -96,13 +75,6 @@ UniqueSplinePtr createPartialTrajectory(
 /// \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
 

src/planner/kunzretimer/KunzRetimer.cpp

@@ -56,44 +56,6 @@ std::unique_ptr<Path> convertToKunzPath(
   return path;
 }
 
-//==============================================================================
-std::unique_ptr<Path> convertToKunzPath(
-    const aikido::trajectory::Spline& traj, double maxDeviation)
-{
-  auto stateSpace = traj.getStateSpace();
-  auto metaSkeletonStateSpace
-      = std::dynamic_pointer_cast<const MetaSkeletonStateSpace>(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<Eigen::VectorXd> waypoints;
-
-  stateSpace = trajectory->getStateSpace();
-  Eigen::VectorXd tmpVec(stateSpace->getDimension());
-  auto tmpState = stateSpace->createState();
-  for (std::size_t i = 0; i < trajectory->getNumWaypoints(); i++)
-  {
-    trajectory->getWaypoint(i, tmpState);
-    stateSpace->logMap(tmpState, tmpVec);
-    waypoints.push_back(tmpVec);
-  }
-
-  auto path = make_unique<Path>(waypoints, maxDeviation);
-  return path;
-}
-
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> convertToSpline(
     const Trajectory& traj,
@@ -184,42 +146,6 @@ std::unique_ptr<aikido::trajectory::Spline> computeKunzTiming(
   return detail::convertToSpline(trajectory, stateSpace, timeStep, startTime);
 }
 
-//==============================================================================
-std::unique_ptr<aikido::trajectory::Spline> computeKunzTiming(
-    const aikido::trajectory::Spline& inputTrajectory,
-    const Eigen::VectorXd& maxVelocity,
-    const Eigen::VectorXd& maxAcceleration,
-    double maxDeviation,
-    double timeStep)
-{
-  const auto stateSpace = inputTrajectory.getStateSpace();
-  const auto dimension = stateSpace->getDimension();
-
-  if (static_cast<std::size_t>(maxVelocity.size()) != dimension)
-    throw std::invalid_argument("Velocity limits have wrong dimension.");
-
-  if (static_cast<std::size_t>(maxAcceleration.size()) != dimension)
-    throw std::invalid_argument("Acceleration limits have wrong dimension.");
-
-  for (std::size_t i = 0; i < dimension; ++i)
-  {
-    if (maxVelocity[i] <= 0.)
-      throw std::invalid_argument("Velocity limits must be positive.");
-    if (!std::isfinite(maxVelocity[i]))
-      throw std::invalid_argument("Velocity limits must be finite.");
-
-    if (maxAcceleration[i] <= 0.)
-      throw std::invalid_argument("Acceleration limits must be positive.");
-    if (!std::isfinite(maxAcceleration[i]))
-      throw std::invalid_argument("Acceleration limits must be finite.");
-  }
-
-  double startTime = inputTrajectory.getStartTime();
-  auto path = detail::convertToKunzPath(inputTrajectory, maxDeviation);
-  Trajectory trajectory(*path, maxVelocity, maxAcceleration, timeStep);
-  return detail::convertToSpline(trajectory, stateSpace, timeStep, startTime);
-}
-
 //==============================================================================
 KunzRetimer::KunzRetimer(
     const Eigen::VectorXd& velocityLimits,
@@ -250,16 +176,12 @@ std::unique_ptr<aikido::trajectory::Spline> KunzRetimer::postprocess(
 
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> KunzRetimer::postprocess(
-    const aikido::trajectory::Spline& inputTraj,
+    const aikido::trajectory::Spline& /*inputTraj*/,
     const aikido::common::RNG& /*rng*/,
     const aikido::constraint::TestablePtr& /*constraint*/)
 {
-  return computeKunzTiming(
-      inputTraj,
-      mVelocityLimits,
-      mAccelerationLimits,
-      mMaxDeviation,
-      mTimeStep);
+  throw std::runtime_error(
+      "[KunzRetimer] does not support postprocessing spline trajectory.");
 }
 
 //==============================================================================

src/planner/parabolic/ParabolicUtil.cpp

@@ -142,26 +142,8 @@ std::unique_ptr<ParabolicRamp::DynamicPath> convertToDynamicPath(
     bool _preserveWaypointVelocity)
 {
   auto stateSpace = _inputTrajectory.getStateSpace();
-  auto metaSkeletonStateSpace
-      = std::dynamic_pointer_cast<const MetaSkeletonStateSpace>(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<ParabolicRamp::Vector> milestones;
   std::vector<ParabolicRamp::Vector> velocities;
   milestones.reserve(numWaypoints);
@@ -172,12 +154,12 @@ std::unique_ptr<ParabolicRamp::DynamicPath> convertToDynamicPath(
   for (std::size_t iwaypoint = 0; iwaypoint < numWaypoints; ++iwaypoint)
   {
     auto currentState = stateSpace->createState();
-    trajectory->getWaypoint(iwaypoint, currentState);
+    _inputTrajectory.getWaypoint(iwaypoint, currentState);
 
     stateSpace->logMap(currentState, currVec);
     milestones.emplace_back(toVector(currVec));
 
-    trajectory->getWaypointDerivative(iwaypoint, 1, tangentVector);
+    _inputTrajectory.getWaypointDerivative(iwaypoint, 1, tangentVector);
     velocities.emplace_back(toVector(tangentVector));
   }
 
@@ -202,26 +184,9 @@ std::unique_ptr<ParabolicRamp::DynamicPath> convertToDynamicPath(
     const Eigen::VectorXd& _maxVelocity,
     const Eigen::VectorXd& _maxAcceleration)
 {
-  auto stateSpace = _inputTrajectory.getStateSpace();
-  auto metaSkeletonStateSpace
-      = std::dynamic_pointer_cast<const MetaSkeletonStateSpace>(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();
+  auto trajectory = toR1JointTrajectory(_inputTrajectory);
+  auto stateSpace = trajectory->getStateSpace();
+  const auto numWaypoints = trajectory->getNumWaypoints();
 
   std::vector<ParabolicRamp::Vector> milestones;
   std::vector<ParabolicRamp::Vector> velocities;

src/trajectory/util.cpp

@@ -118,25 +118,6 @@ UniqueSplinePtr convertToSpline(const Interpolated& inputTrajectory)
   return outputTrajectory;
 }
 
-//==============================================================================
-UniqueInterpolatedPtr convertToInterpolated(
-    const Spline& traj, statespace::ConstInterpolatorPtr interpolator)
-{
-  auto stateSpace = traj.getStateSpace();
-  auto outputTrajectory
-      = make_unique<Interpolated>(stateSpace, std::move(interpolator));
-
-  auto state = stateSpace->createState();
-  for (std::size_t i = 0; i < traj.getNumWaypoints(); ++i)
-  {
-    traj.getWaypoint(i, state);
-    const double t = traj.getWaypointTime(i);
-    outputTrajectory->addWaypoint(t, state);
-  }
-
-  return outputTrajectory;
-}
-
 //==============================================================================
 UniqueInterpolatedPtr concatenate(
     const Interpolated& traj1, const Interpolated& traj2)
@@ -168,22 +149,6 @@ UniqueInterpolatedPtr concatenate(
   return outputTrajectory;
 }
 
-//==============================================================================
-UniqueSplinePtr concatenate(const Spline& traj1, const Spline& traj2)
-{
-  if (traj1.getStateSpace() != traj2.getStateSpace())
-    throw std::runtime_error("State space mismatch");
-
-  const auto& stateSpace = traj1.getStateSpace();
-  statespace::ConstInterpolatorPtr interpolator
-      = std::make_shared<statespace::GeodesicInterpolator>(stateSpace);
-  auto interpolated1 = convertToInterpolated(traj1, interpolator);
-  auto interpolated2 = convertToInterpolated(traj2, interpolator);
-  auto concatenatedInterpolated = concatenate(*interpolated1, *interpolated2);
-
-  return convertToSpline(*concatenatedInterpolated);
-}
-
 //==============================================================================
 double findTimeOfClosestStateOnTrajectory(
     const Trajectory& traj,
@@ -344,24 +309,5 @@ UniqueInterpolatedPtr toR1JointTrajectory(const Interpolated& trajectory)
   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<statespace::GeodesicInterpolator>(space);
-
-  ConstInterpolatedPtr interpolatedTrajectory
-      = std::move(convertToInterpolated(trajectory, interpolator));
-  auto r1JointTrajectory = toR1JointTrajectory(*interpolatedTrajectory);
-  auto splineTrajectory = convertToSpline(*r1JointTrajectory);
-
-  return splineTrajectory;
-}
-
 } // namespace trajectory
 } // namespace aikido