Can't set a pose goal in Rviz/Moveit! when combining godel package and part of Moveit! tutorial
I'm running on Ubuntu 16.0.4 with ROS kinetic installed.
Project background
I have an engineering background with not that much programming experience so I'm sorry if I assume some things that are wrong.
My goal is to make a robot scan a box and cut through the middle of it. For my project I don't have to use an actual scanner to get a point cloud of the box, we implement a virtual point cloud in it instead. To make a "minimal viable product", I want to insert the box at a known location, still make it move around above the box to scan it, and then make it cut through it. Weĺl use a standard size box for now with a known location so I only need it to move from known coordinate A to known coordinate B.
Problem description
I am currently using Godel (https://github.com/ros-industrial-con...) for the scan part so I'm using the following code in the terminal to launch a irb2400 robot with the standard Godel package:
roslaunch godel_irb2400_support irb2400_blending.launch
What I mean by using the scan part is that I use the first step of Godel to make the robot move around above the box where it would usually make the point cloud, but I think this is irrelevant for the problem.
To make the robot move from location A to location B I want to use this part of the Moveit! tutorial page (http://docs.ros.org/kinetic/api/movei...). Specifically, I'm using the move_group_python_interface_tutorial.py file and I've adjusted it to fit my own purposes.
So far most parts of the python script work with both the Panda robot (the one that is in the Moveit! tutorial) and with the irb2400 (the arm from the Godel package) with some minor adjustments. However, the part where it needs to go to a certain "pose" doesn't work for me.
I get the following lines in the terminal I used to launch the Godel package
[ INFO] [1573566726.873179897]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1573566726.874144183]: Planning attempt 1 of at most 1
[ INFO] [1573566726.878820828]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1573566726.880605002]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1573566731.884029276]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1573566731.884102184]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1573566731.884158985]: No solution found after 5.004028 seconds
[ INFO] [1573566731.907918805]: Unable to solve the planning problem
[ INFO] [1573566731.908854657]: Received event 'stop'
and in the Moveit! tutorial script terminal I get the following before it shuts down:
[ INFO] [1573566731.908515315]: ABORTED: No motion plan found. No execution attempted.
I've tried to isolate the problem in my script by cutting away some parts that are irrelevant without causing other problems ...
Are you sure the pose is valid and also reachable? I'm not too familiar with the error messages yet, but 'Unable to sample any valid states for goal tree' seems to indicate the pose has either no valid IK solution at all or the tolerances are too strict (as also encountered here).
I think I have located where the problem lies, but I don't have a practical solution for it yet. When I set the orientation tolerance to a rather big value, it will find a solution. However it tends to twist the end effector to a rather odd position. I think that my lack of understanding of the quaternion orientation might play a role in here too because this seems to rotate the tools, while when I use cartesian coordinates it keeps the tools straight which is what I want.
For now the cartesian coordinates are a work around, but saying "go from point A to B without rotating the tools would be a more viable solution.
Sounds like the pose is invalid. With large enough tolerances it will always find a solution, but this is clearly not the intent of the tolerances here ;) (The intent is to account for the mismatch in target pose and calculated pose while using numbers with finite accuracy).
If you just want a translation, why not get the current pose, edit the x,y,z coordinates and use reuse this as the target pose?