Adds SE2BoxConstraint #135

gilwoolee · 2017-02-10T22:15:21Z

This PR adds:

SE2BoxConstraint which is Testable, Sampleable, and Projectable.
helper methods in JoinStateSpaceHelpers-Impl.hpp so that dart's SE2 joints with x, y constraints can use be tested, sampled, and projected.
SE2 distance metric distance::SE2 which takes l2 norm of (dx, dy, dw). (Happy to take other suggestions too.)

To be consistent with SO2 and SO3 samplers, this does not allow limit on rotational joint, but only on x, y dimensions.

This change is

mkoval · 2017-02-13T00:47:13Z

Why did you close this? These look like a great set of changes.

gilwoolee · 2017-02-13T15:47:46Z

I realized that I wanted to make some changes, e.g. making the distance function weighted sum of radial distance and R^2 norm , and add at least some basic tests. Not sure I could do it in the next a few days (given IROS deadline), so I'll re open it as soon as its ready! Thanks for noticing. - Gilwoo

…

On Sun, Feb 12, 2017 at 7:47 PM, Michael Koval ***@***.***> wrote: Why did you close this? These look like a great set of changes. — You are receiving this because you modified the open/close state. Reply to this email directly, view it on GitHub <#135 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AE7LaGHTVR8psOgKlBNhZNZnsGm_pxbdks5rb6gRgaJpZM4L96dl> .

gilwoolee · 2017-04-04T02:23:02Z

This is not demo-priority but we'll need it eventually for base planning. I haven't made any changes I promised, but it'd be great if someone could take a quick pass and comment/agree on the general structure.

Conflicts: include/aikido/distance/detail/defaults-impl.hpp

codecov-io · 2017-05-16T15:37:17Z

Codecov Report

❗ No coverage uploaded for pull request base (master@00480ed). Click here to learn what that means.
The diff coverage is 54.54%.

@@            Coverage Diff            @@
##             master     #135   +/-   ##
=========================================
  Coverage          ?   73.19%           
=========================================
  Files             ?      148           
  Lines             ?     4092           
  Branches          ?      632           
=========================================
  Hits              ?     2995           
  Misses            ?      650           
  Partials          ?      447

Impacted Files	Coverage Δ
src/statespace/SO3.cpp	`83.87% <100%> (ø)`
src/statespace/SE2.cpp	`76.05% <100%> (ø)`
include/aikido/distance/detail/defaults-impl.hpp	`66.66% <100%> (ø)`
...constraint/uniform/detail/RnBoxConstraint-impl.hpp	`69.69% <100%> (ø)`
.../constraint/detail/JointStateSpaceHelpers-impl.hpp	`57.6% <16.66%> (ø)`

brianhou

Changes look good overall. It'd be great if you have time to add the weighted norm and tests, but it might be easier to address it in a future PR and merge this now!

brianhou · 2017-06-01T17:58:38Z

include/aikido/constraint/detail/JointStateSpaceHelpers-impl.hpp

+      throw std::invalid_argument(
+        "Rotational component of SE2Joint must not have limits.");
+    }
+    else if (!joint->hasPositionLimit(1) && !joint->hasPositionLimit(2))


Should this condition be !(joint->hasPositionLimit(1) && joint->hasPositionLimit(2))?

brianhou · 2017-06-01T17:59:51Z

include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp

@@ -146,7 +146,7 @@ RBoxConstraint<N>::RBoxConstraint(std::shared_ptr<statespace::R<N>> _space,
    {
      std::stringstream msg;
      msg << "Unable to sample from StateSpace because lower limit exeeds"


Nit: exeeds -> exceeds

brianhou · 2017-06-01T18:22:32Z

src/constraint/uniform/SE2BoxConstraint.cpp

+  , mRnDimension(2)
+  , mDimension(3)
+{
+  mLowerLimits[0] = -M_PI;


Nit: can we explicitly document that the limits are [theta, x, y]? It seems that this is a result of SE2::logMap.

👍 to adding documentation for this.

Also, I wonder why we don't take the bounds for the rotation angle of SE(2) here. @gilwoolee Do we have a reason not to do so?

@jslee02 was this documented somewhere? I don't seem to see it.

Never mind, found it!

brianhou · 2017-06-01T18:27:05Z

src/distance/SE2.cpp

+    const aikido::statespace::StateSpace::State* _state1,
+    const aikido::statespace::StateSpace::State* _state2) const
+{
+  Eigen::VectorXd tangent1;


Should these be Vector3d?

VectorXd is correct here. logMap, which is a virtual function of StateSpace, takes VectorXd and resize it if needed.

jslee02 · 2017-06-01T19:36:13Z

It'd be great if you have time to add the weighted norm and tests, but it might be easier to address it in a future PR and merge this now!

👍 If we merge this without those great changes, then it would be less great to add issues to track this issues. 😄

aditya-vk · 2017-06-26T19:26:28Z

tests/planner/ompl/test_MotionValidator.cpp

@@ -29,7 +29,7 @@ class MotionValidatorTest : public ::testing::Test
        std::make_shared<MockTranslationalRobotConstraint>(
            stateSpace, Eigen::Vector3d(-0.1, -0.1, -0.1),
            Eigen::Vector3d(0.1, 0.1, 0.1));
-    auto vchecker =
+    ::ompl::base::StateValidityCheckerPtr vchecker =


@jslee02 not specifying explicitly the type was resulting in a build error due to ambiguity of the variable type, between StateValidityCheckerPtr and StateValidityCheckerFn. I thought you might want to confirm this change.

aditya-vk · 2017-06-26T19:27:57Z

If we merge this without those great changes, then it would be less great to add issues to track this issues.

Introduced the weighted norm and added tests. Please do review. Thanks!

jslee02

@aditya-vk Thanks for adding the tests.

I've made some style fixes and one minor but the necessary fix in computing SE2 distance (see the comment below).

As we have two distance metric classes, I would like to suggest changing the name of Weighted class to something like CartesianProductWeighted. For the backward compatibility, we could also alias Weighted to the new class name with a warning when a user tries to use Weighted.

To do this:
(1) create CartesianProductWeighted.hpp/cpp files and move class Weighted to them renaming the class name
(2) remove Weighted.cpp
(3) remove the original code in Weighted.hpp and alias Weighted to CartesianProductWeighted
(4) add the warning preprocessor directive #warning Weighted is deprecated. Please use CartesianProductWeighted instead by including CartesianProductWeighted.hpp.

jslee02 · 2017-09-19T20:45:15Z

src/distance/SE2Weighted.cpp

+  if (angularDistance > M_PI)
+  {
+    angularDistance -= 2.0 * M_PI;
+    angularDistance = -angularDistance;


@aditya-vk I added this line to make angularDistance to be always positive. Previously, angluarDistance could be negative if we get in this if-statement.

jslee02 · 2017-09-19T21:06:35Z

Unrelated issue: It seems the Travis tests on ubuntu isn't working due to http://packages.personalrobotics.ri.cmu.edu is down. Let me resolve this by switching to using PPA instead of running our own debian package server, as discussed.

jslee02 · 2017-09-20T17:14:12Z

@aditya-vk After I changed the logic of SE2Weighted::distance(), SE2BoxConstraintTests::createSampleGenerator started to fail. I could make the test pass by increasing DISTANCE_THRESHOLD from 0.15 to 1.5, but am not sure if this is a proper change. Could you take a look at this as well? Thanks!

aditya-vk · 2017-09-20T18:59:11Z

src/distance/SE2Weighted.cpp

-    angularDistance -= 2.0 * M_PI;
-    angularDistance = -angularDistance;
-  }
+    angularDistance = 2*M_PI - angularDistance;


@jslee02 thanks for the change! Simplified it into a single statement.

aditya-vk · 2017-09-20T19:00:21Z

tests/constraint/test_SE2BoxConstraint.cpp

@@ -24,7 +24,7 @@ class SE2BoxConstraintTests : public ::testing::Test
  static constexpr size_t NUM_X_TARGETS { 10 };
  static constexpr size_t NUM_Y_TARGETS { 10 };
  static constexpr size_t NUM_SAMPLES { 1000 };
-  static constexpr double DISTANCE_THRESHOLD { 0.15 };
+  static constexpr double DISTANCE_THRESHOLD { 0.8 };


@jslee02 For the number of samples taken and the resolution, the maximum distance between a sample and a target in this case is ~0.74. increasing the threshold to 0.8 is sensible in my opinion.

aditya-vk · 2017-09-20T22:13:08Z

@jslee02 I agree, the Weighted class needs to be renamed. Pushed the relevant changes and issued a warning message suggesting using the new class, in the Weighted.hpp file. Thanks!

jslee02 · 2017-09-21T00:45:08Z

include/aikido/distance/Weighted.hpp

-
-} // namespace distance
-} // namespace aikido
+#warning Weighted is deprecated. Please use CartesianProductWeighted instead by including "CartesianProductWeighted".



Please add using Weighted = CartesianProductWeighted.

jslee02

LGTM! 👍

Adds SE2 constraint

dc493c5

gilwoolee requested a review from jslee02 February 10, 2017 22:15

gilwoolee closed this Feb 10, 2017

gilwoolee reopened this Apr 4, 2017

Merge branch 'master' into SE2constraint

ca0a0c0

gilwoolee requested a review from aditya-vk April 4, 2017 02:22

gilwoolee self-assigned this Apr 4, 2017

jslee02 and others added 7 commits April 10, 2017 22:07

Merge branch 'master' into SE2constraint

fe1c182

Merge branch 'master' into SE2constraint

ce374d6

Fix build errors and warnings

801326c

Merge remote-tracking branch 'origin/master' into SE2constraint

337e715

Conflicts: include/aikido/distance/detail/defaults-impl.hpp

Fix minor styles issues

757352e

More style update

3481930

Use slightly better Eigen operators accordingly

c573de5

jslee02 mentioned this pull request May 16, 2017

Code format: all #211

Merged

gilwoolee requested review from mkoval and brianhou May 22, 2017 20:50

Merge branch 'master' into SE2constraint

4486492

brianhou approved these changes Jun 1, 2017

View reviewed changes

brianhou assigned aditya-vk Jun 12, 2017

jslee02 mentioned this pull request Jun 12, 2017

Add tests for SE2BoxConstraint #215

Closed

jslee02 and others added 4 commits June 12, 2017 19:51

Fix typo

cf3d06d

Fix conversion between SE(2) and 3-dim vector

1454a93

Address Brian's comments

85878fa

add se2BoxConstraint header

9b37286

Aditya Vamsikrishna added 2 commits June 26, 2017 12:31

tests for SE2BoxConstraint

22376f6

add more tests for SE2BoxConstraint

3b2c7c8

aditya-vk reviewed Jun 26, 2017

View reviewed changes

brianhou assigned jslee02 Sep 18, 2017

Merge branch 'master' into SE2constraint

4e6e26d

jslee02 added this to the Aikido 0.1.0 milestone Sep 18, 2017

jslee02 added 5 commits September 19, 2017 10:27

Fix code format

3257249

Fix style

86b03a3

Fix SE2Weighted::distance to return always positive distance

a094477

Try to use DART 6.1

2f3b2cb

Try to use DART 6.1 (reverted from commit 2f3b2cb)

d525eb8

jslee02 suggested changes Sep 19, 2017

View reviewed changes

simplified logic statements; fixed threshold

efc96e3

aditya-vk reviewed Sep 20, 2017

View reviewed changes

Aditya Vamsikrishna added 4 commits September 20, 2017 12:20

remove weighted.cpp

99204b9

introduce cartesianproduct files and make changes in affected files

c44447e

Edit affected test files

69f7802

warn that weighted is deprecated

156baef

jslee02 suggested changes Sep 21, 2017

View reviewed changes

Aditya Vamsikrishna added 2 commits September 21, 2017 17:07

alias

ee789b1

include namespace for weighted

920d75f

jslee02 approved these changes Sep 23, 2017

View reviewed changes

brianhou merged commit 846ea27 into master Sep 23, 2017

brianhou deleted the SE2constraint branch September 23, 2017 02:02

aditya-vk mentioned this pull request Oct 9, 2018

Follow DART SE(2) convention #458

Merged

5 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Adds SE2BoxConstraint #135

Adds SE2BoxConstraint #135

gilwoolee commented Feb 10, 2017 •

edited by mkoval

Loading

mkoval commented Feb 13, 2017

gilwoolee commented Feb 13, 2017 via email

gilwoolee commented Apr 4, 2017

codecov-io commented May 16, 2017 •

edited by codecov bot

Loading

brianhou left a comment

brianhou Jun 1, 2017

brianhou Jun 1, 2017

brianhou Jun 1, 2017

jslee02 Jun 13, 2017

brianhou Sep 23, 2017

brianhou Sep 23, 2017

brianhou Jun 1, 2017

jslee02 Jun 13, 2017

jslee02 commented Jun 1, 2017

aditya-vk Jun 26, 2017

aditya-vk commented Jun 26, 2017

jslee02 left a comment

jslee02 Sep 19, 2017

jslee02 commented Sep 19, 2017

jslee02 commented Sep 20, 2017 •

edited

Loading

aditya-vk Sep 20, 2017

aditya-vk Sep 20, 2017

aditya-vk commented Sep 20, 2017 •

edited

Loading

jslee02 Sep 21, 2017

jslee02 left a comment

Adds SE2BoxConstraint #135

Adds SE2BoxConstraint #135

Conversation

gilwoolee commented Feb 10, 2017 • edited by mkoval Loading

mkoval commented Feb 13, 2017

gilwoolee commented Feb 13, 2017 via email

gilwoolee commented Apr 4, 2017

codecov-io commented May 16, 2017 • edited by codecov bot Loading

Codecov Report

brianhou 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

jslee02 commented Jun 1, 2017

Choose a reason for hiding this comment

aditya-vk commented Jun 26, 2017

jslee02 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 commented Sep 19, 2017

jslee02 commented Sep 20, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

aditya-vk commented Sep 20, 2017 • edited Loading

Choose a reason for hiding this comment

jslee02 left a comment

Choose a reason for hiding this comment

gilwoolee commented Feb 10, 2017 •

edited by mkoval

Loading

codecov-io commented May 16, 2017 •

edited by codecov bot

Loading

jslee02 commented Sep 20, 2017 •

edited

Loading

aditya-vk commented Sep 20, 2017 •

edited

Loading