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 move_group interface (MoveitCommander class), specifically the output from compute_cartesian_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 /check_state_validity 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 compute_cartesian_path using the python move_group interface that would be great! I'd also be curious if it's possible using c++.
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.