MoveitCommander compute_cartesian_path collision return info
I'm using moveit (latest ROS noetic binaries) on Ubuntu 20.04 LTS. I'm trying to create some logic surrounding failures from moveit's python movegroup interface (MoveitCommander class), specifically the output from computecartesian_path. Currently, if the planning fails, it will return a percent of the plan completed, and the trajectory plan UP TO the point of failure, but not including the point of failure. As such, I don't know the reason for the failure, which I'm trying to use to help re-plan.
For instance, I have a simple path where the robot moves the end-effector down 0.2m in z and then back up 0.2m in z to the original starting place. Depending on the starting place, this move can cause a collision between links 2 and 4 (using a fanuc lrmate200id7l model). I figured out it was a collision failure because I took the last point of the plan and moved to that point, then I had to manually move the goal state around in the rviz moveit motionplanning interface to see what the issue was. If I had the next point in the path, I could potentially use moveit's /checkstatevalidity service to determine if there was a link collision (or other issue), but I only have the point right before.
If anyone has any suggestions on how to determine the failure mode/joint positions at failure of computecartesianpath using the python move_group interface that would be great! I'd also be curious if it's possible using c++.
Asked by emielke on 2022-08-25 16:07:53 UTC
Answers
This capability is what compute_cartesian_path eventually connects to. It uses this section of RobotState, which checks here for collisions (setFromIK returns false if it does not find a collision-free pose).
You could create your own MoveGroupCapability on this basis and return the first failed pose and/or the reason for failure. A PR to add such a verbose mode would likely be welcome, since a response to "Why did this plan fail?" is very useful. Sadly, the reason for planning failures is generally not as easy to answer as in this case.
Asked by fvd on 2022-08-27 06:11:58 UTC
Comments
That is very helpful, thanks for the information. I don't know that I would need a "why did this fail", but having the joint positions would allow me to see where it failed in space, which would be useful to help narrow down between out of workspace, link collisions, part collisions, etc.
I'll look into adding a capability and opening a PR once done.
Asked by emielke on 2022-08-31 15:25:19 UTC
Comments
No, I don't believe there would be a way.
compute_cartesian_path(..)'s contract is rather simple, and exactly as you describe: try to linearly interpolate between two poses, given a maximum step (ie: linear delta), a collision scene, a robot model, associated kinematics and a jump distance (ie: joint space delta). Return is success, or failure, and a path. If failure, partial path. That's it.You could see whether the Moveit Task Constructor provides you with more information, but even then you might have to construct your own RobotState instance, set it to the last pose of the plan, move it further, and check whether it's in collision or something like that.
Asked by gvdhoorn on 2022-08-26 09:37:17 UTC