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

Orientation Constraint lead to Trajectory Discontinuities #562

Open
wmy101 opened this issue Jul 25, 2017 · 5 comments

Comments

@wmy101
Copy link

commented Jul 25, 2017

Hi,

I am able to plan an orientation constraint for my end-effector, being parallel to the ground. However, occasionally the path planned results in the end-effector to randomly flip. Is there a method to remove these rotating motion and keep the end effector parallel at all times?

I am currently using RRTConnectkConfigDefault planner and trac_ik solver. I have set the tolerances for the orientation constraint as 2pi and the other two axis as 0.3.

Thanks,

@v4hn

This comment has been minimized.

Copy link
Member

commented Jul 26, 2017

Please have a look at #541 for an analysis of the problem and a workaround that might or might not help you.
This pull-request has not been released yet.

@wmy101

This comment has been minimized.

Copy link
Author

commented Jul 27, 2017

Hi, I have tried the workaround and I definitely see an improvement. The enforce_joint_model_state_space flag prevented the end effector from going out of the orientation constraint. It also finds the solution reasonably quick (able to use in real time). However, occasionally it would fail the planning (timeout set at 60secs). This happened 4/25 times when I tested it. Is this normal?

Thanks,

@v4hn

This comment has been minimized.

Copy link
Member

commented Jul 28, 2017

Well, the patch I linked to is a workaround, it does not address the root of the problem.
As explained in the other thread, enforcing the joint model state space implies rejection sampling and this can be quite slow if the planner fails to find the valid part of the search space.
I've not experienced the problem this bad, but yes, planning might fail depending on your overall configuration/setup.

I don't know enough about the internals of OMPL to explain why it fails horribly in few cases while it achieves much better performance in most cases. This sounds like full restarts could improve the planning a lot..
Originally I expected a somewhat unimodal distribution for the planning times when asking for the same plan over and over again. Sometimes, this is not the case though, as with the setting you describe.
Maybe @davetcoleman or @mamoll could give some intuition for that behavior?

Things you can consider:

  • pragmatic view: multiple plans with short timeouts
  • precompute the valid search space: if you use a small set of constraints, generate state databases that satisfy the constraints (see here for an example generator)
  • "ideal" solution: fix the PoseModelStateSpace implementation so that non-adjacent states in joint space are not adjacent in pose space.
@mamoll

This comment has been minimized.

Copy link
Contributor

commented Jul 28, 2017

I think @zkingston might know why this is happening. It could be related to some weird Euler angle interpolation issue.

@v4hn v4hn changed the title Orientation Constraint not being constrained Orientation Constraint lead to Trajectory Discontinuities Jan 25, 2018

@MiroslavKohut

This comment has been minimized.

Copy link

commented Feb 13, 2018

I met with the same problem is there any solution for this ? or issue is actual ? Solution of this is very important for my application.

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