moveit
moveit copied to clipboard
ros-planning
Strange MoveIt commander behavior with ROS Noetic with UR robots
Description
I'm using ROS Noetic and moveit commander (move group python interface) with pose targets (using set_pose_target). In most of the time MoveIt is not able to plan any path (see below), and when it does it's a very strange solution. With my same application I had no issues with ROS Melodic, but the issue can be reproduced easily on ROS Noetic using the simulation or the real robot.
[ INFO] [1621454575.663979800, 95.245000000]: Planning attempt 1 of at most 1
[ INFO] [1621454575.666822400, 95.248000000]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1621454575.667156900, 95.248000000]: manipulator/manipulator: Starting planning with 1 states already in datastructure
[ERROR] [1621454580.672333000, 99.602000000]: manipulator/manipulator: Unable to sample any valid states for goal tree
[ INFO] [1621454580.672491800, 99.602000000]: manipulator/manipulator: Created 1 states (1 start + 0 goal)
[ INFO] [1621454580.672859000, 99.602000000]: No solution found after 5.005736 seconds
[ INFO] [1621454580.683008900, 99.607000000]: Unable to solve the planning problem
[ INFO] [1621454580.685616400, 99.609000000]: Received event 'stop'
This issue was originally reported at universal_robot and it was suggested to report it here instead.
First I thought it's an issue with my own application above moveit commander, so I created a very simple vanila test environtment, but the issue still exists without any modification on the original packages! Here are the steps to reproduce:
- git clone -b calibration_devel-staging https://github.com/fmauch/universal_robot
- Change arm_controller to pos_joint_traj_controller in ur3e_moveit_planning_execution.launch
- Launch the following launchfiles:
- roslaunch ur_gazebo ur3e_bringup.launch limited:=true
- roslaunch ur3e_moveit_config ur3e_moveit_planning_execution.launch sim:=true limited:=true
Your environment
- ROS Distro: Noetic
- OS Version: Ubuntu 20.04
- Binary build installed with apt install (ros-noetic-moveit/focal,now 1.1.2-1focal.20210424.071603 amd64 [installed])
- Built from source, master branch (commit b2b7e6deb4553d470e3dd9b76c896bae2e068f9f)
- IK solver: KDL
Steps to reproduce
See above
Expected behaviour
MoveIt should be able to plan trajectories to pose targets
Actual behaviour
MoveIt isn't able to plan any path or when it does it looks strange
Backtrace or Console output
See above
Not yet, I didn't built MoveIt from source in the past few years. I checked out the debug log though:
...
[DEBUG] [1621498426.986322400, 134.889000000]: Input: [ -1.11591 2.36105 2.29294 -3.09748 1.56173 2.81371]
[DEBUG] [1621498426.986461400, 134.889000000]: [ 0] delta_twist: [-4.54513e-05,-0.000133777, 5.33314e-05, 0, 0, 0]
[DEBUG] [1621498426.986600000, 134.889000000]: [ 0] pos err: 0.000151 rot err: 0.000000 delta_q: 0.002534
[DEBUG] [1621498426.986944900, 134.889000000]: delta_q: [ 0.000367772 0.000245483 0.000649412 -0.00089823 5.25227e-06 0.00036775]
[DEBUG] [1621498426.987095900, 134.889000000]: q: [ -1.11554 2.3613 2.29359 -3.09838 1.56173 2.81408]
[DEBUG] [1621498426.987185900, 134.889000000]: [ 1] delta_twist: [-5.36739e-08, 5.55038e-09, 8.006e-08, 0, 0, 0]
[DEBUG] [1621498426.987269500, 134.889000000]: Result 0 after 1 iterations: [ -1.11554 2.3613 2.29359 -3.09838 1.56173 2.81408]
[DEBUG] [1621498426.987641000, 134.889000000]: Solved after 0.001517 < 0.005s and 1 attempts
[DEBUG] [1621498426.987787400, 134.889000000]: searchPositionIK: Position request pose is 5.881e-05 0.299943 0.100016 -0.383354 0.923853 0.000413549 8.75237e-05
[DEBUG] [1621498426.987905800, 134.889000000]: Input: [ 3.71726 -1.32648 -1.87569 -0.421315 -0.00042264 -5.31329]
[DEBUG] [1621498426.988025800, 134.889000000]: [ 0] delta_twist: [ -0.283666, 0.381833, -0.350894, 0.962102, 1.27564, 0.547653]
[DEBUG] [1621498426.988124700, 134.889000000]: [ 0] pos err: 0.591092 rot err: 1.689033 delta_q: 5.768056
[DEBUG] [1621498426.988237800, 134.889000000]: delta_q: [ -1.61452 -0.280609 -0.763436 0.197379 2.61182 0.300292]
[DEBUG] [1621498426.988315000, 134.889000000]: q: [ 2.10273 -1.60708 -2.63912 -0.223937 2.61139 -5.01299]
[DEBUG] [1621498426.988397800, 134.889000000]: [ 1] delta_twist: [ -0.0562547, 0.497353, -0.0800319, -1.00693, 0.531549, 1.2458]
[DEBUG] [1621498426.988548200, 134.889000000]: [ 1] pos err: 0.506882 rot err: 1.687743 delta_q: 5.684934
[DEBUG] [1621498426.988591500, 134.889000000]: delta_q: [ 1.24606 1.88443 -0.502469 0.652313 -0.940596 -0.459066]
[DEBUG] [1621498426.988710700, 134.889000000]: q: [ 3.3488 0.277344 -3.14159 0.428376 1.6708 -5.47206]
[DEBUG] [1621498426.988777000, 134.889000000]: [ 2] delta_twist: [ -0.0105308, 0.422235, -0.167923, -0.318326, 2.25879, -0.207432]
[DEBUG] [1621498426.988825900, 134.889000000]: error increased: 1.687743 -> 2.290521, scale: 0.200000
[DEBUG] [1621498426.988876800, 134.889000000]: [ 2] pos err: 0.454524 rot err: 2.290521 delta_q: 1.136987
[DEBUG] [1621498426.989001700, 134.889000000]: delta_q: [ 0.249212 0.376886 -0.100494 0.130463 -0.188119 -0.0918132]
[DEBUG] [1621498426.989090000, 134.889000000]: q: [ 2.35195 -1.2302 -2.73962 -0.0934741 2.42327 -5.10481]
[DEBUG] [1621498426.989223600, 134.889000000]: [ 3] delta_twist: [ -0.074014, 0.462287, -0.127627, -0.703525, 0.778271, 0.801353]
[DEBUG] [1621498426.989291800, 134.889000000]: [ 3] pos err: 0.485259 rot err: 1.320159 delta_q: 9.424678
[DEBUG] [1621498426.989378600, 134.889000000]: delta_q: [ 2.24914 3.48167 -0.401975 -0.791958 -1.71001 0.789923]
[DEBUG] [1621498426.989431300, 134.889000000]: q: [ 4.60109 2.25147 -3.14159 -0.885432 0.713264 -4.31488]
[DEBUG] [1621498426.989544500, 134.889000000]: [ 4] delta_twist: [ -0.193598, 0.374127, -0.104604, -2.49053, 0.758868, 1.14162]
[DEBUG] [1621498426.989737300, 134.889000000]: error increased: 1.320159 -> 2.842870, scale: 0.200000
[DEBUG] [1621498426.990116800, 134.889000000]: [ 4] pos err: 0.434043 rot err: 2.842870 delta_q: 1.884936
[DEBUG] [1621498426.990257900, 134.889000000]: delta_q: [ 0.449828 0.696335 -0.080395 -0.158392 -0.342002 0.157985]
[DEBUG] [1621498426.990398500, 134.889000000]: q: [ 2.80178 -0.533864 -2.82001 -0.251866 2.08127 -4.94682]
[DEBUG] [1621498426.990576800, 134.889000000]: [ 5] delta_twist: [ -0.0738559, 0.417344, -0.171194, -0.572857, 1.03535, 0.505304]
[DEBUG] [1621498426.990780900, 134.889000000]: [ 5] pos err: 0.457097 rot err: 1.286644 delta_q: 28.907451
[DEBUG] [1621498426.990964900, 134.889000000]: delta_q: [ 3.48141 6.81705 -0.32158 -6.03132 -8.36446 3.89163]
[DEBUG] [1621498426.991196200, 134.889000000]: q: [ 6.28319 6.28319 -3.14159 -6.28319 -6.28319 -1.05519]
[DEBUG] [1621498426.991675600, 134.890000000]: [ 6] delta_twist: [ -0.0302912, 0.0767928, -0.137184, -0.280501, -2.07949, 2.0778]
[DEBUG] [1621498426.991726100, 134.890000000]: error increased: 1.286644 -> 2.953003, scale: 0.200000
[DEBUG] [1621498426.991759000, 134.890000000]: [ 6] pos err: 0.160106 rot err: 2.953003 delta_q: 5.781490
[DEBUG] [1621498426.991804200, 134.890000000]: delta_q: [ 0.696282 1.36341 -0.064316 -1.20626 -1.67289 0.778327]
[DEBUG] [1621498426.991847300, 134.890000000]: q: [ 3.49806 0.829546 -2.88433 -1.45813 0.408381 -4.1685]
[DEBUG] [1621498426.991904200, 134.890000000]: [ 7] delta_twist: [ -0.0749896, 0.502034, -0.127161, -0.124851, 1.60152, 1.46657]
[DEBUG] [1621498426.992028200, 134.890000000]: error increased: 1.286644 -> 2.175150, scale: 0.040000
[DEBUG] [1621498426.992122100, 134.890000000]: [ 7] pos err: 0.523289 rot err: 2.175150 delta_q: 1.156298
[DEBUG] [1621498426.992164800, 134.890000000]: delta_q: [ 0.139256 0.272682 -0.0128632 -0.241253 -0.334578 0.155665]
[DEBUG] [1621498426.992205400, 134.890000000]: q: [ 2.94103 -0.261182 -2.83288 -0.493118 1.74669 -4.79116]
[DEBUG] [1621498426.992335600, 134.890000000]: [ 8] delta_twist: [ -0.0713929, 0.431753, -0.162789, -0.443053, 1.07159, 0.70882]
[DEBUG] [1621498426.992632600, 134.892000000]: error increased: 1.286644 -> 1.359056, scale: 0.008000
[DEBUG] [1621498426.992830800, 134.892000000]: [ 8] pos err: 0.466913 rot err: 1.359056 delta_q: 0.231260
[DEBUG] [1621498426.992959700, 134.892000000]: delta_q: [ 0.0278513 0.0545364 -0.00257264 -0.0482506 -0.0669157 0.0311331]
[DEBUG] [1621498426.993755500, 134.893000000]: q: [ 2.82963 -0.479328 -2.82259 -0.300116 2.01436 -4.91569]
[DEBUG] [1621498426.994007300, 134.893000000]: [ 9] delta_twist: [ -0.0735053, 0.419833, -0.169619, -0.546061, 1.03909, 0.54632]
[DEBUG] [1621498426.994222500, 134.893000000]: error increased: 1.286644 -> 1.294742, scale: 0.001600
[DEBUG] [1621498426.994430800, 134.893000000]: [ 9] pos err: 0.458730 rot err: 1.294742 delta_q: 0.046252
[DEBUG] [1621498426.994564200, 134.893000000]: delta_q: [ 0.00557026 0.0109073-0.000514528 -0.00965011 -0.0133831 0.00622661]
[DEBUG] [1621498426.994804800, 134.893000000]: q: [ 2.80735 -0.522957 -2.82053 -0.261516 2.06789 -4.9406]
[DEBUG] [1621498426.994999200, 134.893000000]: [ 10] delta_twist: [ -0.0737943, 0.417825, -0.170884, -0.567472, 1.03596, 0.513519]
[DEBUG] [1621498426.995208200, 134.893000000]: error increased: 1.286644 -> 1.287996, scale: 0.000320
[DEBUG] [1621498426.995255800, 134.893000000]: [ 10] pos err: 0.457411 rot err: 1.287996 delta_q: 0.009250
[DEBUG] [1621498426.995312800, 134.893000000]: delta_q: [ 0.00111405 0.00218146-0.000102906 -0.00193002 -0.00267663 0.00124532]
[DEBUG] [1621498426.995356800, 134.893000000]: q: [ 2.80289 -0.531683 -2.82012 -0.253796 2.0786 -4.94558]
[DEBUG] [1621498426.995459400, 134.896000000]: [ 11] delta_twist: [ -0.0738439, 0.417439, -0.171132, -0.571779, 1.03547, 0.506948]
[DEBUG] [1621498426.995506100, 134.896000000]: error increased: 1.286644 -> 1.286903, scale: 0.000064
[DEBUG] [1621498426.995544600, 134.896000000]: [ 11] pos err: 0.457160 rot err: 1.286903 delta_q: 0.001850
[DEBUG] [1621498426.995596600, 134.896000000]: delta_q: [ 0.00022281 0.000436291-2.05811e-05-0.000386004-0.000535325 0.000249065]
[DEBUG] [1621498426.995718800, 134.896000000]: q: [ 2.802 -0.533428 -2.82003 -0.252252 2.08074 -4.94657]
[DEBUG] [1621498426.995825500, 134.896000000]: [ 12] delta_twist: [ -0.0738535, 0.417363, -0.171182, -0.572641, 1.03538, 0.505633]
[DEBUG] [1621498426.995907900, 134.896000000]: error increased: 1.286644 -> 1.286695, scale: 0.000013
[DEBUG] [1621498426.996026600, 134.896000000]: [ 12] pos err: 0.457110 rot err: 1.286695 delta_q: 0.000370
[DEBUG] [1621498426.996166600, 134.896000000]: delta_q: [ 4.45621e-05 8.72582e-05-4.11622e-06-7.72009e-05-0.000107065 4.98129e-05]
[DEBUG] [1621498426.996232100, 134.896000000]: q: [ 2.80182 -0.533777 -2.82002 -0.251943 2.08117 -4.94677]
[DEBUG] [1621498426.996359800, 134.896000000]: [ 13] delta_twist: [ -0.0738554, 0.417348, -0.171192, -0.572814, 1.03536, 0.50537]
[DEBUG] [1621498426.996447400, 134.896000000]: error increased: 1.286644 -> 1.286654, scale: 0.000003
[DEBUG] [1621498426.996529700, 134.896000000]: [ 13] pos err: 0.457100 rot err: 1.286654 delta_q: 0.000074
[DEBUG] [1621498426.996617500, 134.896000000]: delta_q: [ 8.91241e-06 1.74516e-05-8.23245e-07-1.54402e-05 -2.1413e-05 9.96258e-06]
[DEBUG] [1621498426.996701100, 134.896000000]: q: [ 2.80178 -0.533847 -2.82001 -0.251881 2.08125 -4.94681]
[DEBUG] [1621498426.996748300, 134.896000000]: [ 14] delta_twist: [ -0.0738558, 0.417345, -0.171194, -0.572848, 1.03535, 0.505317]
[DEBUG] [1621498426.996777000, 134.896000000]: error increased: 1.286644 -> 1.286646, scale: 0.000001
[DEBUG] [1621498426.996804300, 134.896000000]: [ 14] pos err: 0.457098 rot err: 1.286646 delta_q: 0.000015
[DEBUG] [1621498426.996839400, 134.896000000]: delta_q: [ 1.78248e-06 3.49033e-06-1.64649e-07-3.08804e-06 -4.2826e-06 1.99252e-06]
[DEBUG] [1621498426.996891700, 134.896000000]: q: [ 2.80178 -0.533861 -2.82001 -0.251869 2.08127 -4.94682]
[DEBUG] [1621498426.996954200, 134.896000000]: [ 15] delta_twist: [ -0.0738559, 0.417344, -0.171194, -0.572855, 1.03535, 0.505307]
[DEBUG] [1621498426.997088000, 134.896000000]: error increased: 1.286644 -> 1.286644, scale: 0.000000
[DEBUG] [1621498426.997208800, 134.896000000]: [ 15] pos err: 0.457098 rot err: 1.286644 delta_q: 0.000003
[DEBUG] [1621498427.009336400, 134.913000000]: Result -2 after 15 iterations: [ 2.80178 -0.533864 -2.82001 -0.251866 2.08127 -4.94682]
[DEBUG] [1621498427.009418600, 134.913000000]: IK timed out after 0.0216405 > 0.005s and 1 attempts
[DEBUG] [1621498427.009545200, 134.913000000]: searchPositionIK: Position request pose is 5.96232e-05 0.300049 0.100057 -0.383218 0.92391 -0.000223232 0.000175073
...
And it seems for me that KDL can find many solutions, the IK sometimes timed out though. But after several thousands of lines I get:
[ERROR] [1621498428.195717600, 135.878000000]: manipulator/manipulator: Unable to sample any valid states for goal tree
[ INFO] [1621498428.196205200, 135.880000000]: manipulator/manipulator: Created 1 states (1 start + 0 goal)
[ INFO] [1621498428.196367000, 135.880000000]: No solution found after 5.026147 seconds
...
[ INFO] [1621498428.233625600, 135.924000000]: Unable to solve the planning problem
[DEBUG] [1621498428.233713000, 135.924000000]: PlanExecution terminating with error code -1 - 'Planning failed.'
[DEBUG] [1621498428.233796000, 135.924000000]: robot state update 1.804
[DEBUG] [1621498428.234035600, 135.924000000]: Setting the current goal as aborted
[DEBUG] [1621498428.234117200, 135.924000000]: robot state update 5.87
[DEBUG] [1621498428.234564400, 135.925000000]: Setting status to aborted on goal, id: /move_group_commander_wrappers_1621498410034612900-2-131.784000000, stamp: 131.78
[DEBUG] [1621498428.234671800, 135.925000000]: Publishing result for goal with id: /move_group_commander_wrappers_1621498410034612900-2-131.784000000 and stamp: 131.78
[DEBUG] [1621498428.234776900, 135.925000000]: Publishing feedback for goal, id: /move_group_commander_wrappers_1621498410034612900-2-131.784000000, stamp: 131.78
[DEBUG] [1621498428.234881500, 135.925000000]: Publishing feedback for goal with id: /move_group_commander_wrappers_1621498410034612900-2-131.784000000 and stamp: 131.78
[ INFO] [1621498428.236090000, 135.927000000]: Received event 'stop'
[DEBUG] [1621498428.264477600, 135.949000000]: performPendingStateUpdate: 8.2644
[DEBUG] [1621498428.264626000, 135.949000000]: robot state update 5.949
[DEBUG] [1621498428.264718600, 135.949000000]: performPendingStateUpdate done
Let me know if there is any debug information that I can provide.
If you're running this right now, could you add pictures of the goal positions/configurations and an example of what you consider "strange"?
Sure, this is an example desired pose:
I cannot reach this if I set a pose target, because I got the error above.
# x, y, z
position = [0.0, 0.3, 0.1]
# x, y, z, w
orientation = [-0.383, 0.924, 0, 0]
def go_to_pose(position, orientation):
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.x = orientation[0]
pose_goal.orientation.y = orientation[1]
pose_goal.orientation.z = orientation[2]
pose_goal.orientation.w = orientation[3]
pose_goal.position.x = position[0]
pose_goal.position.y = position[1]
pose_goal.position.z = position[2]
move_group.set_pose_target(pose_goal)
move_group.go(wait=True)
move_group.stop()
move_group.clear_pose_targets()
However if I randomly pick any result from the debug info generated by MoveIt, let's say:
joint_angles = [1.12489, -1.21653, 2.28989, -2.63392, 4.69404, -4.36674]
def go_to_joint_angles(joint_angles ):
move_group.go(joints, wait=True)
move_group.stop()
MoveIt can easily plan and move the arm to the desired joint angles, and at the end I have the following pose:
pose:
position:
x: -5.428584363175912e-05
y: 0.30009095740275166
z: 0.10003393963756277
orientation:
x: 0.38338013626399725
y: -0.923555963431632
z: -0.006666186637869619
w: 0.004428937093474367
The different signs of the x,y components of the quaternion shouldn't be a problem, because it's the same orientation.
So it seems for me that the issue is somewhere at set_pose_target().
Edit: after running 3 or 4 times the same go_to_pose goal MoveIt executes a plan (same pose input always, but most of the times it fails):
As you can see this is a pose that I call "strange" I never experienced such a result on Melodic.
FYI, @felixvd I just built MoveIt (master branch) from source and I experience the exact same issue.
After changing these to:
roslaunch ur_gazebo ur3e_bringup.launch limited:=true
roslaunch ur3e_moveit_config ur3e_moveit_planning_execution.launch sim:=true limited:=true
this:
roslaunch ur_gazebo ur3_joint_limited.launch
roslaunch ur3_e_moveit_config ur3_e_moveit_planning_execution.launch sim:=true limited:=true
and executing your code on Melodic (master), the behavior appears to be the same as what you described in Noetic. The plan to the first pose target is not always successful, the second can have the alternative configuration you showed, and I never got the third one to work.
But I am not sure that this behavior is a bug, and not just a limitation of the IK solver. Are you sure that planning for these pose goals succeeds on Melodic? Your targets are not exactly easy to reach:
Start:
Goal 1:
Goal 2:
Note how the first goal is near the edge of the robot workspace, and can't be reached within the same configuration (some joints need to flip). If the KDL solver does not reach the goal via gradient descent from the current pose, it re-initializes from a random configuration. Considering how far the goals are from your starting pose, it is not surprising to see joint flips due to this. Your targets may just be too difficult.
Planning to an easier-to-find pose goal succeeds reliably, e.g. position = [0.15, 0.26, 0.45] and orientation = [0,0,0,1]:
Have you tried using another IK solver like TRAC-IK, or an analytical one like IKFast? This may help if you absolutely need these poses.
It is generally a good idea to keep motions with cartesian pose targets within the same joint configuration, avoid moving too far, and moving to a joint goal to be in a known configuration before planning to pose targets.
PS: You can just call roslaunch ur3_e_moveit_config demo.launch instead of starting up gazebo.
Based on the modification that you had to make on the launchfile names, could you please let me know which branch do you use from unversal_robot repo? I'm using a real ur3e not just the Gazebo simulation so I'm using @fmauch's calibration_devel branch that I linked in the first post.
I can agree that the pose target 0, 0.3, 0.1 is a but unlucky, but here is an example with 0, 0.4, 0.1 on melodic:
This is just a very simple script to reproduce the issue to make discussions easier :) I can show you some further evidence that everything is working pretty great on Melodic both in Gazebo and on real robot: https://www.youtube.com/watch?v=m3CO6SK8gzE https://www.youtube.com/watch?v=llD6eGD8nEM https://www.youtube.com/watch?v=BLvH7DzvwUk
I cannot share a video about Noetic, because the arm literally doesn't nothing at all regardless what kind of pose targets do I use. None of the poses can be done by the arm that works on the videos above.
Right, there was an update that I hadn't pulled. I was on commit 00447a of https://github.com/fmauch/universal_robot. I haven't tested the newest commit yet.
I can't look at this further right now, but FWIW I've been using two UR5e robots with that commit and the MoveIt master branch and have had no issue with pose goals (although on Ubuntu 18.04).
I can show you some further evidence that everything is working pretty great on Melodic both in Gazebo and on real robot
By this you mean with the binaries on Melodic, right? Does the same issue occur if you build the master branch on Ubuntu 18.04/ROS Melodic? I wasn't clear before, my bad. Note that you can use a Docker container and this script as well.
On Melodic I'm using binary install of MoveIt (ros-melodic-moveit/bionic 1.0.7-1bionic.20210421.070859 amd64), I'll build MoveIt master from source later, by the way is it possible to use the master on Ubuntu 18.04 and Melodic? Isn't there any issues with python versions?
There's no obstacle to using the master branch on Ubuntu 18.04 and Melodic, as long your nodes use Python 2.7. Using Python 3 in Melodic is a bit more complicated, but that's an unrelated story.
@felixvd, excuse me for the late response, I was busy with other things to do.
This week I had to install Ubuntu 18.04 and ROS Melodic on another machine where we use our Gazebo simulation of the UR3e arm.
I accidentally realized, that 1.0.8 is the latest moveit release on Melodic with apt and 1.0.8 produced a few strange movements compared to the previous 1.0.7. I just built 1.0.7 from source and everything was working fine.
Today I gave it a try and built the 1.0.7 on Noetic and my reported issue wasn't present (not a big surprise though).
Do you have any idea what could cause other strange planning issues between 1.0.7 and 1.0.8 that might be connected to my original issue with 1.1.2. I checked out the commit history and didn't find any commit message that might be a potential reason.
I accidentally realized, that
1.0.8is the latestmoveitrelease on Melodic withaptand1.0.8produced a few strange movements compared to the previous1.0.7. I just built1.0.7from source and everything was working fine.
Ouch. That's obviously not expected and sounds like a severe regression.
Do you have any idea what could cause other strange planning issues between
1.0.7and1.0.8that might be connected to my original issue with1.1.2. I checked out the commit history and didn't find any commit message that might be a potential reason.
If you're planning with a simple scene (no fancy structures, no octomap, no custom access to low-level structures), the only commits that seem related to me are d90acb32d3f27d893bc69252430361e4cc09822f and c4f8fd4f5e48a5ddee0a320b3009b63dd54ba21a .
Could you please test whether reverting these helps to solve your issue? If they don't, please run a git bisect (possibly git bisect run if you can automate the test) so we can identify the offending patch.
Sure thing, let me check. In the meantime, I created a short video about different behaviors of different MoveIt versions: https://www.youtube.com/watch?v=_w2xvUdkQqI
Every node and configuration is the same, only the built MoveIt version is different. As you can see, my expected behavior is what you can see from 00:00 to 01:13 with MoveIt 1.0.7.
MoveIt 1.0.8 behaves pretty strange (doesn't matter if I build it or install with apt) from 01:13 to 03:15.
And finally the MoveIt master (technically 1.1.5+) is an epic fail with my setup from 03:15. Just for the record, I used the master branch of moveit and moveit_msgs and 0.7.3 of geometric_shapes to build it on Ubuntu 18.04 and Melodic.
In the video above all the 3 chapters are 200% speed but you can see that even the joint speeds seems to be strange with other versions than 1.0.7.
@v4hn your gut feeling was perfect, d90acb3 introduced some very nasty movements. If your gut feelings are this good, do you think if I have some rare/special issue in my setup that triggers exoctic corner cases?
@felixvd i didn't test the c4f8fd but I don't expect big difference compared to 1.0.8. I'm not at my workstation, but I'll check it out tomorrow and let you know about the results! Just for the record, I've tested d90acb3 and the previous commit, d90acb3 is nasty, the previous one behaves the same as 1.0.7.
@stuart-fb @tylerjw This is a serious issue introduced by your change. Please have a look into it very soon!
I'm not sure I will find the time for something like this in any depth.
If your gut feelings are this good, do you think if I have some rare/special issue in my setup that triggers exoctic corner cases
It looks like the picked IK solutions for your target poses are not the one you would expect to see. This might be because the change to decide() actually rejects these solutions now.
BTW: The chessboard is a brilliant setup. You could profit a lot from modelling the individual moves with MoveIt Task Constructor. For one thing it would not attempt to execute solutions that move the arm down into the chessboard. :cat2:
@v4hn thanks, and MoveIt Task Constructor seems to be a perfect tool for me, I didn't know it!
@JafarAbdi, that's great! If you'd like to test it in my setup I'm happy to share it!
If you can, please provide a minimum example to reproduce (i.e., one of the robots from moveit_resources without fancy collision geometries). Debugging such issues with a convoluted setup as yours is very impractical.
@v4hn the most simple test setup is that I shared in my 1st post, that doesn't require the gripper and the chessboard.
Just to summarize what we have here, we do have two bugs:
1- In the master branch, there's a bug causing OrientationConstraint::decide to always fail with a very small error bigger than the default tolerance (0.001), it can be reproduced with the following calls
position = [0.0, 0.3, 0.1]
orientation = [-0.383, 0.924, 0, 0]
go_to_pose(position, orientation)
# or
position = [0.131, 0.307, 0.5299]
orientation = [-0.800, 0.331, 0.191, 0.461]
go_to_pose(position, orientation)
Using git bisect I was able to find the culprit commit which is https://github.com/ros-planning/moveit/pull/1964, applying the following diff fixes the bug (@dudasdavid do you mind testing it .?), I'll look again into this maybe later in the weekend
diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
index 55ed6a618..33894abd0 100644
--- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
+++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
@@ -500,8 +500,8 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q
// diff is isometry by construction
// getDesiredRotationMatrix() returns a valid rotation matrix by contract
// reqr has thus to be a valid isometry
- Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear());
- quat = Eigen::Quaterniond(reqr.linear()); // reqr is isometry, so quat has to be normalized
+ Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation());
+ quat = Eigen::Quaterniond(reqr.rotation()); // reqr is isometry, so quat has to be normalized
// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
// model
@@ -510,8 +510,8 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q
// getFrameTransform() returns a valid isometry by contract
const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
// rt is isometry by construction
- Eigen::Isometry3d rt(t.linear() * quat);
- quat = Eigen::Quaterniond(rt.linear()); // rt is isometry, so quat has to be normalized
+ Eigen::Isometry3d rt(t.rotation() * quat);
+ quat = Eigen::Quaterniond(rt.rotation()); // rt is isometry, so quat has to be normalized
}
}
else
2- In melodic & noetic there's a bug that causes wired movement, @dudasdavid found out that it's related to https://github.com/ros-planning/moveit/commit/d90acb32d3f27d893bc69252430361e4cc09822f
For the second bug, I wasn't able to debug it since the first one took me almost a whole day
@JafarAbdi I'll check out your mentioned diff on the master branch during the weekend and let you know!
I cannot share a video about Noetic, because the arm literally doesn't nothing at all regardless what kind of pose targets do I use. None of the poses can be done by the arm that works on the videos above.
1- In the master branch, there's a bug causing OrientationConstraint::decide to always fail with a very small error bigger than the default tolerance (0.001), it can be reproduced with the following calls
That explains why the arm does not accept any goal in master for this demo. If the proposed patch actually solves the problem we will have to renormalize some rotation matrix/quaternion.
Looking further through the code, Orientation constraints check their input quaternion's norm with an epsilon of 1e-3. The two example quaternions you gave above are only normalized up to 2e-4 and 7e-4, respectively.
As the rotation matrices resulting from these are linearly reweighted by the random samples, converted back to quaternion and later (for decide()) to RPY or AngleAxis, I'm not surprised 1e-3 can be exceeded in any dimension of the resulting representations...
TLDR: Instead of renormalizing in the heavy-load code, I'd propose to renormalize the user-specified quaternion during constraint configuration and only throw the current error if the input norm is off by a large margin or close to zero. I don't know whether that fixes the original problem though.
In melodic & noetic there's a bug that causes weird movement, @dudasdavid found out that it's related to d90acb3
After you explained to me that the commit actually fixes some cases where OrientationConstraint::decide rejected quaternions it should have accepted, I'm not sure the original issue reported here is a straight MoveIt problem. But I did not look at it further than watching the screencast above.
If the pipeline by chance generates IK solutions first that would have failed the constraint before because of the bug then these IK solutions might actually turn out valid now and yield inappropriate joint states.
The usual ways to fix this would be to use a different IK solver (trac_ik with distance mode, ur_kinematics or bio_ik with gd_c mode) and/or consider multiple IK solutions using, e.g., MoveIt Task Constructor.
If that's the actual error, it's more that @dudasdavid got quite lucky their pipeline worked before :sun_behind_large_cloud:
But again, it might be something different.
@JafarAbdi, @v4hn I tested the master branch with the proposed diff. It's significantly better because it's able to plan paths, but again, I see very strange decisions several times. I still don't have better word than strange :-) I recorded a video, where normal movements have 6x speedup, only the strange parts go with 1x speed: https://youtu.be/VTp_YkQs9po
As you can see strange movements usually happen when a simple TCP up/down movement is needed. My favourite part is from 2:55.
Is there any way to give some rules for the decision making, e.g. force linear TCP movement without using cartesian plans?
It's significantly better because it's able to plan paths
So @JafarAbdi's patch resolves the first issue he mentioned. I would prefer to isolate the normalization problem further though and fix it at its root. normalizing rotations in the proposed place reintroduces the exact overhead we wanted to get rid of in the original PR he linked.
I still don't have better word than strange
It's either KDL generating the worse solutions first or the planner finding a solution trajectory to the worse solution first even when the better IK solution also exists as a goal.
The most obvious issue to me is that the ur3e config uses the KDL solver, although the ur_kinematics driver could also support the E series and patches exist.
So you can try to change the IK solver (I gave other examples above already), or you restrict the joint limits to avoid inappropriate solutions. If you limit the pan-joint to keep the lift link on one side, and the lift joint to point upwards that basically also gets rid of your problem.
@felixvd, excuse me for the late response, I was busy with other things to do. This week I had to install Ubuntu 18.04 and ROS Melodic on another machine where we use our Gazebo simulation of the UR3e arm. I accidentally realized, that
1.0.8is the latestmoveitrelease on Melodic withaptand1.0.8produced a few strange movements compared to the previous1.0.7. I just built1.0.7from source and everything was working fine.Today I gave it a try and built the
1.0.7on Noetic and my reported issue wasn't present (not a big surprise though).Do you have any idea what could cause other strange planning issues between
1.0.7and1.0.8that might be connected to my original issue with1.1.2. I checked out the commit history and didn't find any commit message that might be a potential reason.
Hi, dudasdavid，I am working on kuka robot now and have met the same question that it cannot find solution when sending it some normal pose commands. I am wondering if it's because i used apt-get to install moveit. If building moveit from source will fix this problem now? or installing moveit 1.0.7 is better (If this way is better, https://moveit.ros.org/install/source/ in this website I only found the instruction of how to install the latest version, if I want to install 1.0.7, downloading moveit 1.0.7 from github, then put it in src dir is the correct way?)
Hi @Loya0598, I don't need this workaround anymore, the problem was fixed in the UR robot's moveit package. However if you want to give it a try, you just check out 1.0.7 tag of moveit and the corresponding tags of moveit_msgs and geometric_shapes and rebuild your workspace.