fix geodesic for SO(2) #408

aditya-vk · 2018-05-10T17:53:09Z

This PR restricts the SO(2) state representation to lie between +pi and -pi.

This ensures planning, for instance with SnapPlanner, between two angles gives the geodesic.
Is similar to other statespace representations [bijective with transformation matrix representation].

This will possibly require keeping a track of another parameter to know what phase of rotation the joint is in?

Looking for proposals and suggestions on handling this in the cleanest possible way [while keeping in mind the trajectories sent to the robot].

~~(Did not modify tests yet.)~~

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

aditya-vk · 2018-05-10T20:08:58Z

src/planner/SnapPlanner.cpp

@@ -37,8 +37,12 @@ trajectory::InterpolatedPtr planSnap(
    }
  }

+  // Redefine the goal state


~This shouldn't be necessary, will be removed.

mkoval · 2018-05-11T02:01:00Z

I don't think this is a good solution to the stated problem.

This ensures planning, for instance with SnapPlanner, between two angles gives the geodesic.

There is no mapping from SO(2) from R that has this property.

Is similar to other statespace representations [bijective with transformation matrix representation].

I don't see any similarity here. The whole point of the Aikido StateSpace class is to make our algorithms independent of our choice of parameterization. We do this by operating on opaque State objects using Lie group operations, rather than operating on raw Eigen vectors.

Our current implementation of GeodesicInterpolator looks good to me. I don't see why it would behave incorrectly in SO(2). What is the behavior that you are seeing?

gilwoolee · 2018-05-11T03:48:00Z

I don't agree with Aditya's current proposal, but I can discuss the problem we have with the current SO2 implementation.

I also agree with @mkoval that there's nothing wrong with current GeodesicInterpolator; it's just the SO2 space.

Here's the problem with our current SO2 implementation. We keep double mAngle as our internal representation, and for inverse, compose and logMap we simply take -mAngle, mAngle1 + mAngle2 and mAngle. If we kept Matrix2d rot as our representation, the operation would've been rot^-1, rot1 * rot2, logMap(rot).

Suppose we do geodesic interpolation from SO2(2*pi) to SO2(0). This should be a no-travel. Consider the halfway point. The tangent vector we get for from->to direction in our current operation is -2pi, so the half-way point ends up being pi.

Here are two tests that fail with the current implementation:
https://github.com/personalrobotics/aikido/blob/bug/SO2/tests/statespace/test_SO2.cpp#L125
https://github.com/personalrobotics/aikido/blob/bug/SO2/tests/statespace/test_SO2.cpp#L150

I am ok with the first one (getAngle inequality), but the 2nd one is definitely an issue.

Maybe I'm misunderstanding @mkoval 's comment here:

There is no mapping from SO(2) from R that has this property.

aditya-vk · 2018-05-11T05:25:12Z

I do not think there is anything wrong with the GeodesicInterpolator either. As @gilwoolee mentioned, the problem's with the SO(2) statespace. The underlying representation is currently an unbounded scalar value representing the angle of rotation while we expect the angle to wrap-around. The issue is in determining the tangentVector which turns out to be target - source in the current implementation, without regard to the right direction.

There are two ways to solve this right.

We could clamp the angle to always lie between -pi and pi, which will lead to loss of some information, but we are guaranteed that it'll be sound with the logic in the geodesic interpolator i.e the direction of interpolation will always be correct. To add to Gilwoo's example, consider the case when my source and target are pi/6 and 11pi/6 (~ -pi/6). The current implementation would interpolate along 11pi/6 - pi/6 while it needs to interpolate the other way. [ This has been my running example :) ]. By clamping it we will have interpolated along -pi/6 - (pi/6) which is what's expected.
We could use a cleaner rotation matrix representation to do the same thing but I am yet to completely trust Eigen's Rotation2D.angle() for that [shall do]. Eigen 3.3 introduced Rotation2D.smallestAngle() which made me wary of the former, hence the quick proposal with the clamp which achieves the same thing.
a) Eigen 3.2 which is the default for Trusty:
http://eigen.tuxfamily.org/dox-3.2/classEigen_1_1Rotation2D.html
b) Eigen 3.3:
http://eigen.tuxfamily.org/dox/classEigen_1_1Rotation2D.html
I do not have a deep understanding about the exp/logMap, at least not as much, but according to @jslee02 and @gilwoolee, the fix might need to be in logMap. I had a brief discussion with @jslee02 about it this evening and he had thoughts I could not wrap my head around at the moment. Maybe he can explain this part?

@gilwoolee thanks for the tests! I should have started with that :)

jslee02 · 2018-05-11T16:20:08Z

The whole point of the Aikido StateSpace class is to make our algorithms independent of our choice of parameterization.

This is true. The only problem I can see is SO2::logMap(). The current implementation (not of this PR) returns the parameter (i.e., rotation angle) as it is, which can lead to none geodesic interpolation in some cases.

Essentially the output of logMap(R) encodes the tangential direction at the origin of SO(2) to travel from identity to R so that R = exp(t*logMap(R)), t = 1, and GeodesicInterpolator simply interpolates from a point to a point following the direction. That said, logMap(R) should return the "geodesic" tangent vector to achieve geodesic behavior from using GeodesicInterpolator.

The criteria for logMap of SO(2) I think is that:

the length of the image is 2pi (e.g., [0, 2*pi) or (-pi, pi]) so that it becomes bijective,
the range of the image is in (-pi, pi] (or [-pi, pi)) so that one can travel to any R through geodesic.

For example, the range of [0, 2*pi) would make GeodesicInterpolator always interpolates counterclockwise even for rotation from 0 to -pi/6, which is not geodesic.

Also, I think the current function names of get/setAngle can confuse people because it sounds like angle is a properties so that the user can get the exactly same angle as it's set. However, as @mkoval pointed out, SO2 is parameter agnostic. That means which representation to use is the implementation detail (it can be 2x2 matrix or angle with different ranges), and setAngle is just a helper function to set SO(2) from angle, which means it sets a SO(2) state using a angle. It'd be good if the function name denotes the differences (setting property vs converting from an angle to a SO(2) point). Similarly, getAngle needs to be changed, and which range to return is our choice (or we can specify which range to use).

All putting together, here is my proposal:

Change SO2::logMap() to return a scalar in (-pi, pi].
Change the name of SO2::setAngle() to SO2::fromAngle().
Change the name of SO2::getAngle() to SO2::toAngle().
Change SO2::toAngle() to return a scalar in (-pi, pi] documenting this explicitly.
[optional] Add option to SO2::toAngle() that specifies the range the user want.

aditya-vk · 2018-05-11T17:47:46Z

Thanks @jslee02 ! 👍
The implementation in this PR currently handles 1 and 4 which ensures the correct behavior. Let me change the function names for more clarity, and add some tests for us to be convinced some corner cases aren't missed. :)

codecov · 2018-05-12T01:30:11Z

Codecov Report

Merging #408 into master will increase coverage by 0.11%.
The diff coverage is 93.54%.

@@            Coverage Diff             @@
##           master     #408      +/-   ##
==========================================
+ Coverage   79.56%   79.68%   +0.11%     
==========================================
  Files         231      231              
  Lines        5955     5960       +5     
==========================================
+ Hits         4738     4749      +11     
+ Misses       1217     1211       -6

Impacted Files	Coverage Δ
src/distance/SO2Angular.cpp	`100% <ø> (ø)`	⬆️
include/aikido/statespace/SO2.hpp	`100% <ø> (ø)`	⬆️
src/constraint/uniform/SO2UniformSampler.cpp	`100% <100%> (ø)`	⬆️
src/statespace/dart/SO2Joint.cpp	`100% <100%> (ø)`	⬆️
include/aikido/statespace/detail/SO2-impl.hpp	`100% <100%> (ø)`	⬆️
src/statespace/SO2.cpp	`94.2% <92.45%> (+9.82%)`	⬆️

aditya-vk · 2018-05-12T02:03:50Z

Updated for further discussion. 👍

jslee02

The current implementation looks good to me. I have some nitpicks though.

Also, it'd be great if you could add high-level comments to the tests.

jslee02 · 2018-05-14T15:52:03Z

include/aikido/statespace/SO2.hpp

@@ -71,78 +34,108 @@ class SO2 : virtual public StateSpace
  /// \return new \c ScopedState
  ScopedState createState() const;

-  /// Gets state as a rotation angle.
+  /// Returns state as a rotation angle.


Nit: Let's describe the return angle is in the range of (-pi, pi].

jslee02 · 2018-05-14T15:53:18Z

include/aikido/statespace/SO2.hpp

-  /// \param _angle rotation angle
-  void setAngle(State* _state, double _angle) const;
+  /// \param[in] angle Rotation angle.
+  /// \param[out] state State corresponding to angle


Nit: Let's order the argument docstring as the corresponding arguments are ordered.

jslee02 · 2018-05-14T15:54:50Z

include/aikido/statespace/SO2.hpp

-  /// \return rotation angle
-  double getAngle(const State* _state) const;
+  /// \param[in] state State.
+  double toAngle(const State* state) const;

  /// Sets state to a rotation angle.


Nit: Let's update the description something like "Sets state from a rotation angle.".

jslee02 · 2018-05-14T15:55:31Z

include/aikido/statespace/SO2.hpp


-  /// Gets state as an Eigen transformation.
+  /// Returns state as an Eigen transformation.


Nit: Eigen rotation.?

jslee02 · 2018-05-14T15:56:15Z

include/aikido/statespace/SO2.hpp

-  void setRotation(State* _state, const Eigen::Rotation2Dd& _rotation) const;
+  /// \param[in] rotation Eigen transformation.
+  /// \param[out] state State corresponding to rotation.
+  void fromRotation(State* state, const Eigen::Rotation2Dd& rotation) const;


Let's order the docstring as the argument listed.

jslee02 · 2018-05-14T15:59:13Z

include/aikido/statespace/SO2.hpp

+  ~State() = default;
+
+  /// Returns state as a rotation angle.
+  double toAngle() const;


Nit: Let's inform that the range of the return angle.

jslee02 · 2018-05-14T15:59:30Z

include/aikido/statespace/SO2.hpp

+  void fromRotation(const Eigen::Rotation2Dd& rotation);
+
+private:
+  double mAngle;


Nit: Please add docstring.

jslee02 · 2018-05-14T16:07:27Z

src/statespace/SO2.cpp

-  if (_state1 == _out || _state2 == _out)
-    throw std::invalid_argument("Output aliases input.");
+#ifndef NDEBUG // Debug mode
+  assert(state1 != out && state2 != out);


I don't know why we wanted to disable this in release mode, but it seems we need this precondition check (using exception) in both modes according to the docstring.

Good catch 👍

jslee02 · 2018-05-14T16:10:00Z

src/statespace/SO2.cpp

-    throw std::invalid_argument("Output aliases input.");
+#ifndef NDEBUG // Debug mode
+  assert(out != in);
+#endif


Ditto with SO2::compose().

jslee02 · 2018-05-14T16:12:05Z

src/statespace/SO2.cpp

-  }
+#ifndef NDEBUG // Debug mode
+  assert(tangent.rows() == 1);
+#endif


Hm, the original warning message seems useful. Let's keep it.

aditya-vk · 2018-05-14T21:02:06Z

Sure. I addressed the minor comments. I'll add a few more tests for catching invalid arguments and notify by removing the WIP label. Thanks!

brianhou · 2018-05-14T21:28:58Z

src/statespace/SO2.cpp

+  double boundedAngle = std::fmod(angle, 2.0 * M_PI);
+  if (boundedAngle > M_PI)
+    boundedAngle -= 2.0 * M_PI;
+  if (boundedAngle < -M_PI)


It looks like this permits boundedAngle to be -pi, while the documentation says that it is bounded in (-pi, pi].

brianhou · 2018-05-14T21:30:27Z

src/statespace/SO2.cpp

-  if (_out == _in)
-    throw std::invalid_argument("Output aliases input.");
+  if (out == in)
+    throw std::invalid_argument("Output aliases input");


Undo deleted period?

gilwoolee

Looks good overall. Added some minor comments.

gilwoolee · 2018-05-14T22:01:48Z

include/aikido/statespace/SO2.hpp

-  /// \param _angle rotation angle
-  void setAngle(State* _state, double _angle) const;
+  /// \param[out] state State corresponding to angle
+  /// \param[in] angle Rotation angle.


Can we specify the range for angle here? (If it allows (-inf, inf), mention that)

gilwoolee · 2018-05-14T22:03:48Z

include/aikido/statespace/SO2.hpp

+
+  /// Sets state from a rotation angle.
+  ///
+  /// \param[in] angle Rotation angle


Same as above, specify the range for angle.

gilwoolee · 2018-05-14T22:05:15Z

src/distance/SO2Angular.cpp

@@ -25,9 +25,9 @@ double SO2Angular::distance(
    const aikido::statespace::StateSpace::State* _state2) const
 {
  // Difference between angles
-  double diff = mStateSpace->getAngle(
+  double diff = mStateSpace->toAngle(


Nit: Are we correcting the variable names in this PR or not? (_state)

I was doing it only in the SO2.cpp/hpp since I was mostly modifying them for this PR. I think I'll leave this for another PR and just have SO2 stuff cleaned up in code style.

gilwoolee · 2018-05-14T22:07:44Z

tests/statespace/test_SO2.cpp

@@ -8,38 +8,59 @@ using Eigen::Rotation2Dd;

 static constexpr double TOLERANCE{1e-6};

+TEST(SO2, BoundedAngle)
+{
+  SO2::State s1(3 * M_PI_2);


I would add tests to check the exact bounds -pi and pi as well.

jslee02

Looks good to me.

jslee02 · 2018-05-15T23:47:22Z

@aditya-vk Please don't forget to update the changelog for completeness.

mkoval · 2018-05-16T01:23:41Z

👍 the latest round of changes look good to me. Thanks @aditya-vk for the great work and to @jslee02 for the excellent summary of the situation as it stood before this pull request.

* restrict so2 between -pi to pi + cleanup * make format * WIP: changes in progress * proposal complete * cascade updates to function names * test the new changes to SO2 space * cascade update of change in function names * make format * corrections in test_SO2 * correct test_SO2Angular * make format * remove unnecessary code block * correct test in test_metaSkeletonStateSpace * address JS's comments * address Gilwoo's comments * add tests for when functions throw * update log * Fix incorrect merging of previous commit

aditya-vk added 4 commits May 9, 2018 19:31

restrict so2 between -pi to pi + cleanup

8d6485d

make format

83d885a

WIP: changes in progress

eaae064

proposal complete

d56674f

aditya-vk added the pr: discussion For discussion or proposals. If it passes to state of implementation, replace with WIP. label May 10, 2018

aditya-vk requested review from gilwoolee and jslee02 May 10, 2018 17:53

Merge branch 'master' into bugfix/avk/so2

75b8ee3

aditya-vk commented May 10, 2018

View reviewed changes

aditya-vk added this to the Aikido 0.3.0 milestone May 10, 2018

Merge branch 'master' into bugfix/avk/so2

4e80b34

aditya-vk added 9 commits May 11, 2018 11:46

cascade updates to function names

bc9eaae

test the new changes to SO2 space

94971e5

cascade update of change in function names

2aaaf18

make format

413e4c6

corrections in test_SO2

c065eaa

correct test_SO2Angular

0ddd98b

make format

b8106f7

remove unnecessary code block

45bd5a3

correct test in test_metaSkeletonStateSpace

e49ce19

Merge branch 'master' into bugfix/avk/so2

9329c6f

jslee02 reviewed May 14, 2018

View reviewed changes

Merge branch 'master' into bugfix/avk/so2

c75fbe8

aditya-vk added pr: work-in-progress Calls for preliminary review of the PR. Remove when PR is ready for complete review. and removed pr: discussion For discussion or proposals. If it passes to state of implementation, replace with WIP. labels May 14, 2018

address JS's comments

8de5b8f

brianhou reviewed May 14, 2018

View reviewed changes

gilwoolee previously approved these changes May 14, 2018

View reviewed changes

jslee02 and others added 3 commits May 15, 2018 09:19

Merge branch 'master' into bugfix/avk/so2

bc60aa3

address Gilwoo's comments

847f1a3

Merge remote into local

e41999f

aditya-vk dismissed gilwoolee’s stale review via e41999f May 15, 2018 17:53

jslee02 previously approved these changes May 15, 2018

View reviewed changes

aditya-vk removed the pr: work-in-progress Calls for preliminary review of the PR. Remove when PR is ready for complete review. label May 15, 2018

add tests for when functions throw

17d044e

aditya-vk dismissed jslee02’s stale review via 17d044e May 15, 2018 18:13

update log

fa5d475

jslee02 previously approved these changes May 16, 2018

View reviewed changes

Merge branch 'master' into bugfix/avk/so2

91556bf

jslee02 dismissed their stale review via 91556bf May 17, 2018 05:24

Fix incorrect merging of previous commit

3daa903

jslee02 approved these changes May 17, 2018

View reviewed changes

aditya-vk merged commit 2259e04 into master May 17, 2018

aditya-vk deleted the bugfix/avk/so2 branch May 18, 2018 01:42

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix geodesic for SO(2) #408

fix geodesic for SO(2) #408

aditya-vk commented May 10, 2018 •

edited

Loading

aditya-vk May 10, 2018 •

edited

Loading

mkoval commented May 11, 2018

gilwoolee commented May 11, 2018 •

edited

Loading

aditya-vk commented May 11, 2018 •

edited

Loading

jslee02 commented May 11, 2018

aditya-vk commented May 11, 2018 •

edited

Loading

codecov bot commented May 12, 2018 •

edited

Loading

aditya-vk commented May 12, 2018

jslee02 left a comment

jslee02 May 14, 2018

jslee02 May 14, 2018

jslee02 May 14, 2018

jslee02 May 14, 2018

jslee02 May 14, 2018

jslee02 May 14, 2018

jslee02 May 14, 2018

jslee02 May 14, 2018

aditya-vk May 14, 2018

jslee02 May 14, 2018

jslee02 May 14, 2018

aditya-vk commented May 14, 2018

brianhou May 14, 2018 •

edited

Loading

brianhou May 14, 2018

gilwoolee left a comment

gilwoolee May 14, 2018

gilwoolee May 14, 2018

gilwoolee May 14, 2018

aditya-vk May 15, 2018 •

edited

Loading

gilwoolee May 14, 2018

jslee02 left a comment

jslee02 commented May 15, 2018

mkoval commented May 16, 2018


		/// Gets state as an Eigen transformation.
		/// Returns state as an Eigen transformation.

fix geodesic for SO(2) #408

fix geodesic for SO(2) #408

Conversation

aditya-vk commented May 10, 2018 • edited Loading

aditya-vk May 10, 2018 • edited Loading

Choose a reason for hiding this comment

mkoval commented May 11, 2018

gilwoolee commented May 11, 2018 • edited Loading

aditya-vk commented May 11, 2018 • edited Loading

jslee02 commented May 11, 2018

aditya-vk commented May 11, 2018 • edited Loading

codecov bot commented May 12, 2018 • edited Loading

Codecov Report

aditya-vk commented May 12, 2018

jslee02 left a 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 commented May 14, 2018

brianhou May 14, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gilwoolee left a 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 May 15, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 left a comment

Choose a reason for hiding this comment

jslee02 commented May 15, 2018

mkoval commented May 16, 2018

aditya-vk commented May 10, 2018 •

edited

Loading

aditya-vk May 10, 2018 •

edited

Loading

gilwoolee commented May 11, 2018 •

edited

Loading

aditya-vk commented May 11, 2018 •

edited

Loading

aditya-vk commented May 11, 2018 •

edited

Loading

codecov bot commented May 12, 2018 •

edited

Loading

brianhou May 14, 2018 •

edited

Loading

aditya-vk May 15, 2018 •

edited

Loading