-
Notifications
You must be signed in to change notification settings - Fork 516
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
[WIP] Cartesian Tool Path Planning #1946
Conversation
moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
Outdated
Show resolved
Hide resolved
Codecov ReportBase: 50.30% // Head: 50.18% // Decreases project coverage by
Additional details and impacted files@@ Coverage Diff @@
## main #1946 +/- ##
==========================================
- Coverage 50.30% 50.18% -0.12%
==========================================
Files 374 374
Lines 31358 31438 +80
==========================================
+ Hits 15772 15773 +1
- Misses 15586 15665 +79
Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here. ☔ View full report at Codecov. |
The code is now at the point where I can pass in a path constraint, it gets instantiated, and we actually get a plan. However, the plan right now either fails because of tight bounds or succeeds but doesn't honor the tool path at all. There are lots of OMPL interface nuances that are still mysterious to me, and I may need some help with this to head in the right direction vs. just doing things with trial and error. |
@zkingston do you have time to take a look? |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Could you elaborate more on the failure case? e.g., when you say the tolerances are too tight and planning fails. I took a quick look through the code.
moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h
Outdated
Show resolved
Hide resolved
moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
Outdated
Show resolved
Hide resolved
moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
Outdated
Show resolved
Hide resolved
Thanks for looking at this! I don't know if I have anything super specific rather than generally trying to understand how this all works. Some pointed questions:
|
|
Interesting... so is there a way in OMPL to do a " |
I think an idea similar to what you have currently, but using the NN to store the centers of each constraint region (which tile the path similar to what you have currently), and then evaluating the error / jacobian to the closest constraint may work. There isn't a way in OMPL to do a "constraint union" like this - I'm not sure if it even is a well-formed concept. |
Thank you for all this input -- these are helpful things to try out! |
I tried going down this path of individual box constraints, and have something that runs with more reusable code... but I keep getting this at the end.
Interestingly enough, if my path has two points, so it's a single box constraint, everything works perfectly and I can do constrained planning in a straight line. But as soon as I add more points, even if it's just splitting a straight line into two, it fails. Will dig more when I find more time / energy. |
void ToolPathConstraint::function(const Eigen::Ref<const Eigen::VectorXd>& joint_values, | ||
Eigen::Ref<Eigen::VectorXd> out) const | ||
{ | ||
const Eigen::Isometry3d eef_pose = forwardKinematics(joint_values); | ||
|
||
const auto eef_nearest = pose_nn_.nearest(EigenIsometry3dWrapper(eef_pose)); | ||
// std::cout << "Function, nearest: " << eef_nearest.path_idx_ << std::endl; | ||
|
||
const auto bc = box_constraints_.at(eef_nearest.path_idx_); | ||
bc->function(joint_values, out); | ||
} | ||
|
||
void ToolPathConstraint::jacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_values, | ||
Eigen::Ref<Eigen::MatrixXd> out) const | ||
{ | ||
const Eigen::Isometry3d eef_pose = forwardKinematics(joint_values); | ||
const auto eef_nearest = pose_nn_.nearest(EigenIsometry3dWrapper(eef_pose)); | ||
// std::cout << "Jacobian, nearest: " << eef_nearest.path_idx_ << std::endl; | ||
|
||
const auto bc = box_constraints_.at(eef_nearest.path_idx_); | ||
bc->jacobian(joint_values, out); | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
These are the "magic" functions that map the EEF pose to the nearest box constraint for that path segment and compute the standard function/Jacobian for that box constraints.
I think this is close, except that I think the state sampler needs to be configured.
This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete. |
Closing as this seems to be abandoned. Feel free to re-open if you take it back up. |
Description
This reimplements #377 with the suggestions from review and continues the work to make it a complete feature.
Checklist