SO2 to R1 conversion for postprocessing #496

aditya-vk · 2019-01-27T03:51:12Z

The timers assume that the path representation is in R1. When the joints in the skeleton are SO2, we would like them to move along the geodesic and not the other way around. To ensure this happens, we need to appropriately convert the path representation to R1 along the geodesic. THis PR handles this conversion.

Before creating a pull request

Document new methods and classes
Format code with make format

Before merging a pull request

Set version target by selecting a milestone on the right side
Summarize this change in CHANGELOG.md
Add unit test(s) for this change

brianhou

Great start! I have a few questions about the implementation, mainly about whether the restriction to 1-DOF joints is necessary.

include/aikido/trajectory/util.hpp

src/trajectory/util.cpp

src/control/ros/Conversions.cpp

include/aikido/trajectory/util.hpp

codecov · 2019-02-03T21:39:25Z

Codecov Report

Merging #496 into master will decrease coverage by 0.03%.
The diff coverage is 77%.

@@            Coverage Diff             @@
##           master     #496      +/-   ##
==========================================
- Coverage   77.87%   77.83%   -0.04%     
==========================================
  Files         262      262              
  Lines        6712     6768      +56     
==========================================
+ Hits         5227     5268      +41     
- Misses       1485     1500      +15

Impacted Files	Coverage Δ
src/planner/vectorfield/VectorFieldPlanner.cpp	`87.75% <ø> (ø)`	⬆️
include/aikido/statespace/CartesianProduct.hpp	`100% <ø> (ø)`	⬆️
src/control/ros/Conversions.cpp	`78.5% <100%> (+0.38%)`	⬆️
src/planner/parabolic/ParabolicUtil.cpp	`92.59% <72.72%> (-5.28%)`	⬇️
src/trajectory/util.cpp	`87.57% <73.8%> (+0.58%)`	⬆️
src/planner/kunzretimer/KunzRetimer.cpp	`74.4% <73.91%> (-1.97%)`	⬇️
src/planner/ompl/CRRT.cpp	`75.58% <0%> (-0.59%)`	⬇️

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

aditya-vk · 2019-02-08T04:51:33Z

@brianhou I do not think dealing with multiple DOF joint spaces needs to be a part of this PR given that the rest of the stack, especially trajectory execution does not support it. Other comments have been addressed.

brianhou · 2019-02-08T05:01:15Z

include/aikido/trajectory/util.hpp

+/// Converts an interpolated trajectory in the cartesian product space of SO(2)
+/// and R1 joints
+/// to a trajectory in cartesian product space of strictly only R1 joints.
+/// \param[in] space Statespace the input trajectory is in.


Delete this line.

brianhou · 2019-02-08T05:01:34Z

include/aikido/trajectory/util.hpp

@@ -89,6 +89,26 @@ double findTimeOfClosestStateOnTrajectory(
 UniqueSplinePtr createPartialTrajectory(
    const Spline& traj, double partialStartTime);

+/// Converts an interpolated trajectory in the cartesian product space of SO(2)
+/// and R1 joints


This is wrapped weirdly, and things aren't capitalized correctly.

brianhou · 2019-02-08T05:02:08Z

include/aikido/trajectory/util.hpp

+/// \param[in] trajectory Trajectory to be converted.
+/// \return Converted trajectory.
+aikido::trajectory::ConstInterpolatedPtr toR1JointTrajectory(
+    // aikido::statespace::ConstStateSpacePtr& space,


Delete this line.

brianhou · 2019-02-08T05:02:16Z

include/aikido/trajectory/util.hpp

+/// Converts a spline trajectory in the cartesian product space of SO(2) and R1
+/// joints
+/// to a trajectory in cartesian product space of strictly only R1 joints.
+/// \param[in] space Statespace the input trajectory is in.


Delete this line.

brianhou · 2019-02-08T05:02:20Z

include/aikido/trajectory/util.hpp

+/// \param[in] trajectory Trajectory to be converted.
+/// \return Converted trajectory.
+aikido::trajectory::ConstSplinePtr toR1JointTrajectory(
+    // aikido::statespace::ConstStateSpacePtr& space,


Delete this line.

brianhou · 2019-02-08T05:03:09Z

src/control/ros/Conversions.cpp

-  space->logMap(state, tangentVector);
+  space->convertStateToPositions(state, tangentVector);
+
+  auto geodesicInterpolator = std::dynamic_pointer_cast<aikido::statespace::GeodesicInterpolator>(interpolator);


Why don't we take in a GeodesicInterpolator, rather than always dynamic casting it here?

Consistency: Seems to be the norm to pass in the base class and then cast it to the appropriate one.

Flexibility: In case we introduce new interpolator classes and want to use the one that's tied to the trajectory.

I don't have a strong preference, but I'd be inclined to go this route.

In that case, can we replace this with methods that are defined in the Interpolator class?

src/trajectory/util.cpp

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

…into so2_to_r1

aditya-vk · 2019-02-08T21:21:18Z

src/trajectory/util.cpp

+  NonSingularDOF
+};
+
+bool checkStateSpace(const statespace::StateSpace* _stateSpace, AllowJointType type)


@brianhou @gilwoolee :

There are a couple of tests written (planner / trajectory namespaces for example) that instantiate an R2 statespace and use this function during the process. I agree that we should not have two functions doing the same here. This seems to be a reasonable solution for now, especially given we currently have support for multi-DOF joints some places and not others. I would like to have this solution in place with an issue raised to

Change the tests to be cartesian product spaces with multiple R1 joints.
or

Extend support for multi-DOF joint spaces uniformly and remove this enum usage.

Since this method is internal, I am okay with using an enum at this point but would liek to get rid of it soon. What do you guys think?

I would prefer to remove the enum and just use a Cartesian product space of multiple R1 joints in these tests, as part of this PR. Then, we can revisit the multi-DOF joint space if/when it becomes useful in the future.

I agree with Brian. Can we have an issue to track this in case it becomes necessary?

brianhou · 2019-02-11T07:58:23Z

src/control/ros/Conversions.cpp

+        "The interpolator of trajectory should be a GeodesicInterpolator");
+  }
+
+  auto diff = geodesicInterpolator->getTangentVector(prevState, state);


I'm not entirely sure I understand why this change is necessary. So far, my understanding of this PR is:

We plan with SO(2) joints

We convert SO(2) joints to R1, postprocess (because postprocessors only deal with Rn joints), then convert back to SO(2)

What are we trying to do in this function? Is it what I thought was happening in #496 (comment), i.e. basically converting this into an Rn-joint trajectory to send out via ROS?

My main qualm is that it feels wrong to be requiring a specific type of interpolator here. I'm wondering whether we can rewrite this without using GeodesicInterpolator-specific methods, perhaps by actually converting it to an Rn trajectory and then just using logMap?

To my understanding, we are committing to executing the shortest paths i.e. interpolating via the Geodesic anytime we execute a trajectory.

What are we trying to do in this function?

We want to extract waypoints along the trajectory "ensuring that the interpolation happens along the geodesic so that SO2 joint space is handled appropriately".

What is the PR doing?

We plan with SO2 joints

We convert SO2 joints to R1 and time it -> this generates a spline that has been timed appropriately.

When converting to ROS trajectory, each waypoint from this spline is extracted using the trajectory's statespace which is SO2 and hence requiring us to use the previous point to be able to interpolate appropriately, and then use the converted extracted waypoint to construct the ROS trajectory.

Conversion into Rn-joint trajectory and then using logMap would also involve the geodesic interpolator so I am not sure how that would help.

The issue is that the base Interpolator class does not have a getTangentVector() and I don't see why it shouldn't since every derived class would need to support that. So I can alternatively add a virtual function there and if we take the interpolator by reference or obtain it from the trajectory, it should point to the right implementation?

But the problem is we need to be able to support both Interpolated trajectories and Splines. Spline trajectories do not have an interpolator attached to them so we need to use something.

@aditya-vk I didn't fully get the 3rd bullet point. trajectory is already in MetaSkeletonStateSpace so it's a Cartesian product of SO2. In SO2 spline trajectory, why do we need the previous point to interpolate appropriately?

Yes exactly. It is in the Cartesian Product of SO2, call it space. So when we evaluate the trajectory, space is used to retrieve the state (=space->createState()). Although the spline polynomial is correct, when we retrieve it using space, the representation for SO2 joints is converted back to between -pi and pi.

This leaves two options: Either have your trajectory be tied with CartesianProduct space of all R1 joints which can again get messy since we check for MetaSkeletonStateSpace during execution or use the interpolation from the previous point.

I was wondering if we could return evaluatePolynomial() instead of evaluate() since that returns the value of the polynomial which is what we desire. However, again, since the spline segments are solved from 0, we need to know the start state of the segment which is stores in mSegments.startState and that is represented as R + SO2. So this route is pretty much useless as well.

src/control/ros/Conversions.cpp

brianhou · 2019-02-14T19:01:12Z

After discussing with @aditya-vk, it seems that somewhere lower down in the stack would be a better place to convert to R1 trajectories (when necessary). I think this would make for a cleaner abstraction, since we'd be able to reason with the SO(2) joints all the way from planning until we send it off to the controller.

I've created personalrobotics/rewd_controllers#30 to track this, let's clean up this PR to remove the conversion here.

Improve documentation. [skip ci]

gilwoolee added 2 commits January 26, 2019 19:29

Convert SO2 to R1 during smoothing

058bd32

Remove whitespaces

e81c6d6

aditya-vk added this to the Aikido 0.3.0 milestone Jan 27, 2019

aditya-vk requested review from brianhou and gilwoolee January 27, 2019 03:51

gilwoolee added 2 commits January 26, 2019 20:00

Add conversion

505cb35

Merge branch 'master' into so2_to_r1

df10624

brianhou suggested changes Feb 2, 2019

View reviewed changes

brianhou added 2 commits February 2, 2019 22:47

Merge branch 'master' into so2_to_r1

28c2711

Merge branch 'master' into so2_to_r1

96614b7

brianhou and others added 5 commits February 7, 2019 19:32

code format

84a7730

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

code format

4252be9

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

address PR comments

93d0223

propogate changes

8a07ad3

Merge branch 'master' into so2_to_r1

a1e90eb

brianhou suggested changes Feb 8, 2019

View reviewed changes

brianhou and others added 5 commits February 8, 2019 10:36

change iter to const

51fc4dd

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

cleanup

a855ab9

Merge branch 'so2_to_r1' of http://github.com/personalrobotics/aikido …

0154d5c

…into so2_to_r1

make format

76e0420

add enum to handle both cases

8b1a27c

aditya-vk commented Feb 8, 2019

View reviewed changes

use enum to support both types

e495412

brianhou reviewed Feb 11, 2019

View reviewed changes

gilwoolee reviewed Feb 11, 2019

View reviewed changes

src/control/ros/Conversions.cpp Outdated Show resolved Hide resolved

gilwoolee reviewed Feb 14, 2019

View reviewed changes

src/control/ros/Conversions.cpp Outdated Show resolved Hide resolved

gilwoolee and others added 2 commits February 13, 2019 23:55

Merge branch 'master' into so2_to_r1

dbe1966

formatting

896d26e

aditya-vk added 9 commits February 14, 2019 07:00

remove support for multi-dof

dd69ec0

modify kunzretimer test

d646535

complete kunz retimer test rewrite

5764e6d

parabolic smoother

e599229

parabolic timer

d538557

all tests converted

7be2f75

make format

460b466

yet another test

1b0b366

make format

0ff8c80

brianhou added 4 commits February 14, 2019 11:41

Remove hacky SO(2) to R1 conversion.

b8c6b00

Improve documentation. [skip ci]

Update CHANGELOG.

c61a796

Return unique_ptr rather than shared_ptr.

6739410

Add TODO to clean up toR1JointTrajectory usage.

a416403

brianhou approved these changes Feb 16, 2019

View reviewed changes

gilwoolee approved these changes Feb 16, 2019

View reviewed changes

gilwoolee merged commit fe6ba16 into master Feb 16, 2019

gilwoolee deleted the so2_to_r1 branch February 16, 2019 06:07

gilwoolee mentioned this pull request Feb 17, 2019

Demo/nips updates #476

Closed

5 tasks

aditya-vk mentioned this pull request Feb 17, 2019

Continuous Joint Planning Failure #430

Open

brianhou mentioned this pull request Aug 20, 2019

Merging changes from the Summer 2019 ADA Demo #542

Closed

5 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

SO2 to R1 conversion for postprocessing #496

SO2 to R1 conversion for postprocessing #496

aditya-vk commented Jan 27, 2019 •

edited by brianhou

Loading

brianhou left a comment

codecov bot commented Feb 3, 2019 •

edited

Loading

aditya-vk commented Feb 8, 2019

brianhou Feb 8, 2019

brianhou Feb 8, 2019

brianhou Feb 8, 2019

brianhou Feb 8, 2019

brianhou Feb 8, 2019

brianhou Feb 8, 2019

aditya-vk Feb 8, 2019

brianhou Feb 8, 2019

aditya-vk Feb 11, 2019

aditya-vk Feb 8, 2019

brianhou Feb 11, 2019

gilwoolee Feb 11, 2019

brianhou Feb 11, 2019

aditya-vk Feb 11, 2019 •

edited

Loading

gilwoolee Feb 11, 2019 •

edited

Loading

aditya-vk Feb 14, 2019 •

edited

Loading

aditya-vk Feb 14, 2019

aditya-vk Feb 14, 2019 •

edited

Loading

brianhou commented Feb 14, 2019

SO2 to R1 conversion for postprocessing #496

SO2 to R1 conversion for postprocessing #496

Conversation

aditya-vk commented Jan 27, 2019 • edited by brianhou Loading

brianhou left a comment

Choose a reason for hiding this comment

codecov bot commented Feb 3, 2019 • edited Loading

Codecov Report

aditya-vk commented Feb 8, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

aditya-vk Feb 11, 2019 • edited Loading

Choose a reason for hiding this comment

gilwoolee Feb 11, 2019 • edited Loading

Choose a reason for hiding this comment

aditya-vk Feb 14, 2019 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

aditya-vk Feb 14, 2019 • edited Loading

Choose a reason for hiding this comment

brianhou commented Feb 14, 2019

aditya-vk commented Jan 27, 2019 •

edited by brianhou

Loading

codecov bot commented Feb 3, 2019 •

edited

Loading

aditya-vk Feb 11, 2019 •

edited

Loading

gilwoolee Feb 11, 2019 •

edited

Loading

aditya-vk Feb 14, 2019 •

edited

Loading

aditya-vk Feb 14, 2019 •

edited

Loading