moveit icon indicating copy to clipboard operation
moveit copied to clipboard

Published 20 hours ago • verified publisher icon ros-planning

Strange MoveIt commander behavior with ROS Noetic with UR robots

Open dudasdavid opened this issue 3 years ago • 30 comments

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:

  1. git clone -b calibration_devel-staging https://github.com/fmauch/universal_robot
  2. Change arm_controller to pos_joint_traj_controller in ur3e_moveit_planning_execution.launch
  3. 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
  1. Run my example code that can be found here.

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

dudasdavid avatar May 20 '21 07:05 dudasdavid

Do you know if the problem exists if you build from source?

felixvd avatar May 20 '21 08:05 felixvd

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.

dudasdavid avatar May 20 '21 08:05 dudasdavid

If you're running this right now, could you add pictures of the goal positions/configurations and an example of what you consider "strange"?

felixvd avatar May 20 '21 09:05 felixvd

Sure, this is an example desired pose: image

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): image

As you can see this is a pose that I call "strange" I never experienced such a result on Melodic.

dudasdavid avatar May 20 '21 09:05 dudasdavid

FYI, @felixvd I just built MoveIt (master branch) from source and I experience the exact same issue.

dudasdavid avatar May 20 '21 11:05 dudasdavid

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: Screenshot from 2021-05-21 16-50-50

Goal 1: Screenshot from 2021-05-21 16-51-10

Goal 2: Screenshot from 2021-05-21 16-52-07

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]:

Screenshot from 2021-05-21 17-24-57

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.

felixvd avatar May 21 '21 08:05 felixvd

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: moveit2

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.

dudasdavid avatar May 21 '21 09:05 dudasdavid

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.

felixvd avatar May 21 '21 09:05 felixvd

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?

dudasdavid avatar May 21 '21 09:05 dudasdavid

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 avatar May 21 '21 10:05 felixvd

@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.

dudasdavid avatar Jun 15 '21 15:06 dudasdavid

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.

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.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.

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.

v4hn avatar Jun 15 '21 18:06 v4hn

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.

dudasdavid avatar Jun 15 '21 19:06 dudasdavid

@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?

dudasdavid avatar Jun 15 '21 20:06 dudasdavid

That's bad. Did you also test c4f8fd, or is the regression only in d90acb3?

felixvd avatar Jun 16 '21 15:06 felixvd

@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.

dudasdavid avatar Jun 16 '21 16:06 dudasdavid

@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 avatar Jun 16 '21 19:06 v4hn

@v4hn thanks, and MoveIt Task Constructor seems to be a perfect tool for me, I didn't know it!

dudasdavid avatar Jun 18 '21 07:06 dudasdavid

I'll look into this on the weekend or Monday next week

JafarAbdi avatar Jun 18 '21 12:06 JafarAbdi

@JafarAbdi, that's great! If you'd like to test it in my setup I'm happy to share it!

dudasdavid avatar Jun 19 '21 15:06 dudasdavid

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 avatar Jun 21 '21 19:06 v4hn

@v4hn the most simple test setup is that I shared in my 1st post, that doesn't require the gripper and the chessboard.

dudasdavid avatar Jun 21 '21 21:06 dudasdavid

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 avatar Jul 02 '21 14:07 JafarAbdi

@JafarAbdi I'll check out your mentioned diff on the master branch during the weekend and let you know!

dudasdavid avatar Jul 02 '21 14:07 dudasdavid

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.

v4hn avatar Jul 02 '21 22:07 v4hn

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.

v4hn avatar Jul 02 '21 22:07 v4hn

@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?

dudasdavid avatar Jul 03 '21 17:07 dudasdavid

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.

v4hn avatar Jul 03 '21 22:07 v4hn

@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.

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?)

Loya0598 avatar Jun 20 '22 08:06 Loya0598

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.

dudasdavid avatar Jun 23 '22 18:06 dudasdavid