Added proper SO2 handling for Spline conversion

CHANGELOG.md

@@ -57,6 +57,7 @@
   * 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)
   * Updated findTimeOfClosestStateTrajectory to use StateSpace Distance Metric: [#543](https://github.com/personalrobotics/aikido/pull/543)
+  * Add SO2 handling in spline conversions: [#546](https://github.com/personalrobotics/aikido/pull/546)
 
 * Planner
 

src/control/ros/Conversions.cpp

@@ -356,6 +356,8 @@ std::unique_ptr<SplineTrajectory> toSplineJointTrajectory(
   }
 
   // Check that all joints are R1Joint or SO2Joint state spaces.
+  // Add subspaces for spline trajectory creation.
+  std::vector<aikido::statespace::ConstStateSpacePtr> subspaces;
   for (std::size_t i = 0; i < numControlledJoints; ++i)
   {
     auto jointSpace = space->getJointSpace(i);
@@ -372,6 +374,14 @@ std::unique_ptr<SplineTrajectory> toSplineJointTrajectory(
               << " DOFs.";
       throw std::invalid_argument{message.str()};
     }
+    else if (!r1Joint)
+    {
+      subspaces.emplace_back(std::make_shared<aikido::statespace::SO2>());
+    }
+    else
+    {
+      subspaces.emplace_back(std::make_shared<aikido::statespace::R1>());
+    }
   }
 
   if (jointTrajectory.points.size() < 2)
@@ -455,18 +465,17 @@ std::unique_ptr<SplineTrajectory> toSplineJointTrajectory(
 
   // Convert the ROS trajectory message to an Aikido spline.
   // 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();
 
+  // Temporary States
+  auto nextState = compoundSpace->createState();
+  auto invCurrState = compoundSpace->createState();
+  auto diffState = compoundSpace->createState();
+
   const auto& waypoints = jointTrajectory.points;
   for (std::size_t iWaypoint = 1; iWaypoint < waypoints.size(); ++iWaypoint)
   {
@@ -487,6 +496,15 @@ std::unique_ptr<SplineTrajectory> toSplineJointTrajectory(
         unspecifiedMetaSkeletonJoints,
         startPositions);
 
+    // Calculate diffVec for Spline conversion
+    Eigen::VectorXd diffPosition;
+    compoundSpace->expMap(currPosition, currState);
+    compoundSpace->expMap(nextPosition, nextState);
+
+    compoundSpace->getInverse(currState, invCurrState);
+    compoundSpace->compose(nextState, invCurrState, diffState);
+    compoundSpace->logMap(diffState, diffPosition);
+
     // Compute spline coefficients for this polynomial segment.
     const auto nextTimeFromStart = waypoints[iWaypoint].time_from_start.toSec();
     const auto segmentDuration = nextTimeFromStart - currTimeFromStart;
@@ -496,12 +514,11 @@ std::unique_ptr<SplineTrajectory> toSplineJointTrajectory(
         currVelocity,
         currAcceleration,
         segmentDuration,
-        nextPosition - currPosition,
+        diffPosition,
         nextVelocity,
         nextAcceleration,
         numCoefficients);
 
-    compoundSpace->expMap(currPosition, currState);
     trajectory->addSegment(segmentCoefficients, segmentDuration, currState);
 
     // Advance to the next segment.

tests/control/ros/test_Conversions.cpp

@@ -46,6 +46,19 @@ class ConvertJointTrajectoryTests : public testing::Test
     waypoint2.time_from_start = ros::Duration{1.};
     waypoint2.positions.assign({2.});
 
+    // Create a two-waypoint SO2-wrapped trajectory for mSkeleton.
+    mTwoWaypointMessageWrap = trajectory_msgs::JointTrajectory{};
+    mTwoWaypointMessageWrap.joint_names.emplace_back(jointProperties.mName);
+    mTwoWaypointMessageWrap.points.resize(2);
+
+    auto& waypoint1wrap = mTwoWaypointMessageWrap.points[0];
+    waypoint1wrap.time_from_start = ros::Duration{0.};
+    waypoint1wrap.positions.assign({3});
+
+    auto& waypoint2wrap = mTwoWaypointMessageWrap.points[1];
+    waypoint2wrap.time_from_start = ros::Duration{1.};
+    waypoint2wrap.positions.assign({-3});
+
     // 2-Joint skeleton
     mSkeleton2Joints = dart::dynamics::Skeleton::create("2JointSkeleton");
 
@@ -89,6 +102,7 @@ class ConvertJointTrajectoryTests : public testing::Test
   SkeletonPtr mSkeleton;
   std::shared_ptr<MetaSkeletonStateSpace> mStateSpace;
   trajectory_msgs::JointTrajectory mTwoWaypointMessage;
+  trajectory_msgs::JointTrajectory mTwoWaypointMessageWrap;
 
   SkeletonPtr mSkeleton2Joints;
   std::shared_ptr<MetaSkeletonStateSpace> mStateSpace2Joints;
@@ -183,6 +197,48 @@ TEST_F(ConvertJointTrajectoryTests, LinearTrajectory)
   EXPECT_EIGEN_EQUAL(make_vector(0.), values, kTolerance);
 }
 
+TEST_F(ConvertJointTrajectoryTests, LinearWrapTrajectory)
+{
+  const auto linearTwoWaypointMessage = mTwoWaypointMessageWrap;
+  const auto trajectory
+      = toSplineJointTrajectory(mStateSpace, linearTwoWaypointMessage);
+
+  ASSERT_TRUE(!!trajectory);
+  ASSERT_DOUBLE_EQ(0., trajectory->getStartTime());
+  ASSERT_DOUBLE_EQ(1., trajectory->getEndTime());
+  EXPECT_DOUBLE_EQ(1., trajectory->getDuration());
+  EXPECT_EQ(1, trajectory->getNumSegments());
+  EXPECT_EQ(1, trajectory->getNumDerivatives());
+
+  auto state = mStateSpace->createState();
+  Eigen::VectorXd values;
+
+  double pose = 3.0 + (M_PI - 3.0) / 2.0;
+  double length = (M_PI - 3.0) * 2.0;
+
+  trajectory->evaluate(0., state);
+  mStateSpace->convertStateToPositions(state, values);
+  EXPECT_EIGEN_EQUAL(make_vector(3.), values, kTolerance);
+
+  trajectory->evaluate(0.25, state);
+  mStateSpace->convertStateToPositions(state, values);
+  EXPECT_EIGEN_EQUAL(make_vector(pose), values, kTolerance);
+
+  trajectory->evaluate(0.75, state);
+  mStateSpace->convertStateToPositions(state, values);
+  EXPECT_EIGEN_EQUAL(make_vector(-pose), values, kTolerance);
+
+  trajectory->evaluate(1., state);
+  mStateSpace->convertStateToPositions(state, values);
+  EXPECT_EIGEN_EQUAL(make_vector(-3.), values, kTolerance);
+
+  trajectory->evaluateDerivative(0.5, 1, values);
+  EXPECT_EIGEN_EQUAL(make_vector(length), values, kTolerance);
+
+  trajectory->evaluateDerivative(0.5, 2, values);
+  EXPECT_EIGEN_EQUAL(make_vector(0.), values, kTolerance);
+}
+
 TEST_F(ConvertJointTrajectoryTests, CubicTrajectory)
 {
   auto cubicTwoWaypointMessage = mTwoWaypointMessage;