Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

continous joint limits are always satisfied #729

Merged

Conversation

Projects
None yet
4 participants
@dcconner
Copy link
Contributor

commented Dec 21, 2017

Description

Some continuous joints (e.g. joint1 on my Kinova Mico robot arm) will show as violating joint limits in some situations.

This change returns true that continuous joints always satisfiesPositionBounds()

Originally, the urdf limits were set to -2PI to +2PI. According to the Urdf Joint documentation, the limits should not be set for continuous joints; however, removing the limits gave the same joint violations in the moveit RViz window for some continuous joints.

One could argue to set the limits to MIN_FLOAT and MAX_FLOAT, but that will not be defined with the "+/-margin" calculation in this file.

Thus, always returning true seems to me to be the safest for continuous joints.

Checklist

  • Required: Code is auto formatted using clang-format
  • Extended the tutorials / documentation, if necessary reference
  • Include a screenshot if changing a GUI
  • Optional: Created tests, which fail without this PR reference
  • Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)
@dcconner

This comment has been minimized.

Copy link
Contributor Author

commented Dec 21, 2017

Travis tests fail at:

EXPECT_FALSE(state.satisfiesBounds(model->getJointModel("joint_a")));

Thus we will need to update this test as part of this pull request. I'll wait for discussion of whether this is good idea before modifying the test.

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Dec 21, 2017

Internally, continuous joints use limits -pi..pi to allow proper sampling in joint space.
Making satisfiesPositionBounds() return true for continuous joints seems to be reasonable to me. However, Ioan explicitly removed this semantics four years ago. Maybe it was just a copy-and-paste error, as the respective commit deals with API changes primarily.

@dcconner

This comment has been minimized.

Copy link
Contributor Author

commented Dec 26, 2017

@isucan - can you comment on this request in light of the commit referenced by @rhaschke above?

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Dec 27, 2017

As @rhaschke alluded, I believe OMPL uses the satisfiesPositionBounds() function and changing this behavior will likely have very large affects on MoveIt!'s core planning capabilities since you will be increasing the configuration space infinitely, which takes... a long time... to search.

The correct behavior is what you are proposing in this PR but before we can accept this we need to ensure OMPL does not rely on this (I'm pretty sure it does) and if it does, change OMPL's MoveIt! interface to implement this pi wrapping functionality at a different level.

@dcconner

This comment has been minimized.

Copy link
Contributor Author

commented Dec 27, 2017

I looked it up, and can confirm that this is used by the ompl interface in state_validity_checker.cpp.

Generating infinite sampling spaces is bad, but one might argue that having continuity of values within a trajectory might be desirable as well.

I originally looked into this due to feedback error on visualization. It may be that there is an easier change on the system driver that will fix that issue, but it does seem that satisfiesPositionBounds is being used in multiple locations for sometimes conflicting purposes.

If I was generating a spline for a continuous joint, I would not want an arbitrary +/- pi bound, but adding a separate method for sampling might be problematic as well.

It seems this needs a larger discussion and potential changes to the OMPL interface before this could be approved.

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Dec 29, 2017

I don't think this PR increases the sampling space. Internally, the joint limits are still -pi ... +pi.
It's just that the bounds checker always returns true for continuous joints.
@davetcoleman Which scenario you have in mind, where OMPL might fail?

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Mar 29, 2018

Internally, the joint limits are still -pi ... +pi.

Can you link to the code you are referencing here?

I'm worried than an OMPL random sampler will be affected by this change, though I'm not entirely sure if that fear is warranted. A good OMPL test here would go a long way.

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented May 26, 2018

Ping, @davetcoleman.
@dcconner, Can you please rebase to resolve conflicts?

@davetcoleman

This comment has been minimized.

Copy link
Member

commented May 29, 2018

I think I was being overly cautious, this change is fine by me

@dcconner

This comment has been minimized.

Copy link
Contributor Author

commented May 30, 2018

I'll look into rebasing tomorrow at work.

@v4hn

This comment has been minimized.

Copy link
Member

commented May 30, 2018

I approve these changes too, assuming you fix the conflicts.

We should only merge this after the upcoming kinetic release, but that's planned for this week anyway.

@dcconner dcconner force-pushed the CNURobotics:fix_continuous_limits branch from ab68f67 to 9504da7 May 30, 2018

@dcconner

This comment has been minimized.

Copy link
Contributor Author

commented May 30, 2018

Rebased and pushed.

I had to manually update the ros class_loader package on my machine to get latest to compile

@@ -168,7 +168,11 @@ double RevoluteJointModel::distance(const double* values1, const double* values2

bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
{
return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
if (!continuous_)

This comment has been minimized.

Copy link
@rhaschke

rhaschke May 30, 2018

Collaborator

The other way around, it would be more readable, wouldn't it?

if (continuous_) return true
else ...
@@ -168,11 +168,12 @@ double RevoluteJointModel::distance(const double* values1, const double* values2

bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
{
if (!continuous_)
if (continuous_)

This comment has been minimized.

Copy link
@rhaschke

rhaschke May 30, 2018

Collaborator

@dcconner Thanks for considering my comment.
Here, the curly brackets could/should be dropped. Otherwise clang enforces very verbose code...

This comment has been minimized.

Copy link
@dcconner

dcconner May 31, 2018

Author Contributor

OK. I see the clang issue. I'm going to rebase again, clean up, push to verify that everything passes, and then re-do this as a single squashed commit and do a force push to avoid the commit spam.

This comment has been minimized.

Copy link
@rhaschke

rhaschke May 31, 2018

Collaborator

Thanks for taking care of a clean commit history. In this case, however, that's not necessary as github allows us to squash-merge everything on top of the current HEAD anyway.

@dcconner dcconner force-pushed the CNURobotics:fix_continuous_limits branch from 0b4a184 to 4bc562c May 31, 2018

@rhaschke rhaschke merged commit 35f842f into ros-planning:kinetic-devel May 31, 2018

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

rhaschke added a commit to ubi-agni/moveit that referenced this pull request May 31, 2018

dg-shadow added a commit to shadow-robot/moveit that referenced this pull request Jul 30, 2018

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.