SO2 to R1 conversion for postprocessing (#496)

* Convert SO2 to R1 during smoothing

* Remove whitespaces

* Add conversion

* code format

Co-Authored-By: aditya-vk <[email protected]>

* code format

Co-Authored-By: aditya-vk <[email protected]>

* address PR comments

* propogate changes

* change iter to const

Co-Authored-By: aditya-vk <[email protected]>

* 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

@@ -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
 

include/aikido/control/ros/Conversions.hpp

@@ -4,8 +4,8 @@
 #include <memory>
 #include <sensor_msgs/JointState.h>
 #include <trajectory_msgs/JointTrajectory.h>
-#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
-#include <aikido/trajectory/Spline.hpp>
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "aikido/trajectory/Spline.hpp"
 
 namespace aikido {
 namespace control {

include/aikido/statespace/CartesianProduct.hpp

@@ -7,6 +7,8 @@
 namespace aikido {
 namespace statespace {
 
+AIKIDO_DECLARE_POINTERS(CartesianProduct)
+
 // Defined in detail/CartesianProduct.hpp
 template <class>
 class CompoundStateHandle;

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
 

src/control/ros/Conversions.cpp

@@ -1,14 +1,15 @@
-#include <aikido/control/ros/Conversions.hpp>
+#include "aikido/control/ros/Conversions.hpp"
 
 #include <sstream>
 #include <unordered_set>
 #include <dart/dynamics/Joint.hpp>
-#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>
+#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<std::pair<std::size_t, std::size_t>>& 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<double>& 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<std::pair<std::size_t, std::size_t>>& indexMap,
+    const std::vector<std::size_t>& 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<const MetaSkeletonStateSpace>& space,
+    const aikido::trajectory::ConstTrajectoryPtr& trajectory,
+    double timeFromStart,
+    trajectory_msgs::JointTrajectoryPoint& waypoint);
+
+//==============================================================================
+void reorder(
+    const std::vector<std::pair<std::size_t, std::size_t>>& indexMap,
+    const Eigen::VectorXd& inVector,
+    Eigen::VectorXd& outVector)
+{
+  assert(indexMap.size() == static_cast<std::size_t>(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<std::pair<std::size_t, std::size_t>>& indexMap,
-    const Eigen::VectorXd& inVector,
-    Eigen::VectorXd& outVector)
-{
-  assert(indexMap.size() == static_cast<std::size_t>(inVector.size()));
-  outVector.resize(inVector.size());
-  for (auto index : indexMap)
-    outVector[index.second] = inVector[index.first];
-}
-
 } // namespace
 
 //==============================================================================
@@ -374,8 +453,18 @@ std::unique_ptr<SplineTrajectory> toSplineJointTrajectory(
     numCoefficients = 2; // linear
 
   // Convert the ROS trajectory message to an Aikido spline.
-  std::unique_ptr<SplineTrajectory> trajectory{new SplineTrajectory{space}};
-  auto currState = space->createState();
+  // Add a segment to the trajectory.
+  std::vector<aikido::statespace::ConstStateSpacePtr> subspaces;
+  for (std::size_t citer = 0; citer < space->getDimension(); ++citer)
+  {
+    subspaces.emplace_back(std::make_shared<aikido::statespace::R1>());
+  }
+
+  auto compoundSpace
+      = std::make_shared<const aikido::statespace::CartesianProduct>(subspaces);
+  std::unique_ptr<SplineTrajectory> 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<SplineTrajectory> 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;
 

src/planner/kunzretimer/KunzRetimer.cpp

@@ -1,11 +1,22 @@
 #include "aikido/planner/kunzretimer/KunzRetimer.hpp"
 #include <dart/dart.hpp>
-#include <aikido/common/Spline.hpp>
-#include <aikido/common/StepSequence.hpp>
+#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,36 +26,71 @@ namespace detail {
 std::unique_ptr<Path> convertToKunzPath(
     const aikido::trajectory::Interpolated& traj, double maxDeviation)
 {
-  std::list<Eigen::VectorXd> waypoints;
   auto stateSpace = traj.getStateSpace();
+  auto metaSkeletonStateSpace
+      = std::dynamic_pointer_cast<const MetaSkeletonStateSpace>(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<Eigen::VectorXd> 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<Path>(waypoints, maxDeviation);
+  auto path = make_unique<Path>(waypoints, maxDeviation);
   return path;
 }
 
 //==============================================================================
 std::unique_ptr<Path> convertToKunzPath(
     const aikido::trajectory::Spline& traj, double maxDeviation)
 {
-  std::list<Eigen::VectorXd> waypoints;
   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 < 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<Path>(waypoints, maxDeviation);
+  auto path = make_unique<Path>(waypoints, maxDeviation);
   return path;
 }
 
@@ -133,7 +179,6 @@ std::unique_ptr<aikido::trajectory::Spline> computeKunzTiming(
   }
 
   double startTime = inputTrajectory.getStartTime();
-
   auto path = detail::convertToKunzPath(inputTrajectory, maxDeviation);
   Trajectory trajectory(*path, maxVelocity, maxAcceleration, timeStep);
   return detail::convertToSpline(trajectory, stateSpace, timeStep, startTime);