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.

Sign up for GitHub

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

Jump to bottom

Avoid invalid argument exceptions in computeJointVelocityFromTwist #262

Merged
merged 7 commits into from
Nov 23, 2017
Merged

Avoid invalid argument exceptions in computeJointVelocityFromTwist #262

merged 7 commits into from
Nov 23, 2017

Conversation

dqyi11
Copy link
Contributor

@dqyi11 dqyi11 commented Nov 20, 2017

Add initial guess (using current MetaSkeleton velocities) to NloptSolver in aikido::planner::vectorfield::computeJointVelocityFromTwist.
Update initial guess according to velocity bounds to avoid invalid argument thrown from nlopt.
This is a fix to #257 .

Before creating a pull request

  • Document new methods and classes (N/A)
  • 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 (N/A)

@dqyi11 dqyi11 added this to the Aikido 0.2.0 milestone Nov 20, 2017
@dqyi11 dqyi11 requested review from brianhou and jslee02 November 20, 2017 19:35
brianhou
brianhou suggested changes Nov 20, 2017
View reviewed changes
src/planner/vectorfield/VectorFieldUtil.cpp Outdated
((velocityUpperLimits[i] - _padding) - position) / _timestep);
velocityUpperLimit,
((positionUpperLimit - _padding) - position) / _timestep);
initialGuess[i] = std::min(velocityUpperLimits[i], initialGuess[i]);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe we can use std::clamp instead?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std::clamp seems a feature since c++17.

src/planner/vectorfield/VectorFieldPlanner.cpp
{
std::rethrow_exception(terminationError);
return convertToSpline(knots, cacheIndex, _stateSpace);
}
else
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does this ever happen? We need to make sure to avoid throwing exceptions from these planning methods. I think we should be printing some warning and returning nullptr instead.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It is possible but not currently reproducible in current test cases.
It is a good idea to throw warning instead. I think all this part of code will be changed in a new version VFP.

src/planner/vectorfield/VectorFieldPlanner.cpp
@@ -196,29 +198,24 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
&& terminationStatus
!= VectorFieldPlannerStatus::CACHE_AND_TERMINATE);

if (cacheIndex >= 0)
// Print the termination condition.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this refactor actually correct? This was a somewhat hasty change to stop throwing exceptions.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes. I think the logic is the same with previous one. The difference is that no exception will be thrown as long as a planned trajectory is returned.

@codecov
Copy link

codecov bot commented Nov 20, 2017 •
edited
Loading

Codecov Report

Merging #262 into master will increase coverage by 0.04%.
The diff coverage is 50%.

@@            Coverage Diff             @@
##           master     #262      +/-   ##
==========================================
+ Coverage   70.39%   70.43%   +0.04%     
==========================================
  Files         181      182       +1     
  Lines        5394     5399       +5     
  Branches      847      847              
==========================================
+ Hits         3797     3803       +6     
+ Misses       1077     1076       -1     
  Partials      520      520
Impacted Files Coverage Δ
src/planner/vectorfield/VectorFieldPlanner.cpp 50.86% <10%> (+0.43%) ⬆️
include/aikido/common/detail/algorithm-impl.hpp 100% <100%> (ø)
src/planner/vectorfield/VectorFieldUtil.cpp 82.6% <57.89%> (+0.25%) ⬆️

@jslee02 jslee02 self-assigned this Nov 22, 2017
@jslee02
Add clamp() function
8139745 
and try to improve the readability of computeJointVelocityFromTwist()
jslee02
jslee02 approved these changes Nov 23, 2017
View reviewed changes
Copy link
Member

@jslee02 jslee02 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good to me.

I added clamp() function and tried to improve the readability of computeJointVelocityFromTwist().

src/planner/vectorfield/VectorFieldPlanner.cpp Outdated
@@ -127,11 +127,13 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
{
if (!_vectorFiledCb(_stateSpace, t, dq))
{
dtwarn << "Failed evaluating VectorField." << std::endl;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why do we need additional warning message when we throw with the same message?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good catch. This part will be rewritten in the new vector field planner. Let's fix that in the new one?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we simply remove this in this PR then?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes. It is moved now.

@brianhou brianhou merged commit fd57586 into master Nov 23, 2017
@brianhou brianhou deleted the bugfix/dqyi/boxConstraintInitialization branch November 23, 2017 08:01
@dqyi11 dqyi11 mentioned this pull request Nov 26, 2017
Closed
gilwoolee pushed a commit that referenced this pull request Jan 21, 2019
@dqyi11 @brianhou
Avoid invalid argument exceptions in computeJointVelocityFromTwist (#262
4de5bba 
)

* add initial guess for nlopt

* code formatting

* update CHANGELOG.md

* Add clamp() function

and try to improve the readability of computeJointVelocityFromTwist()

* remove unncessary warning
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@jslee02 jslee02 jslee02 approved these changes

@brianhou brianhou brianhou approved these changes

Assignees

@jslee02 jslee02

Labels
None yet
Projects
None yet
Milestone
Aikido 0.2.0
Development

Successfully merging this pull request may close these issues.
3 participants
@dqyi11 @brianhou @jslee02