Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ur_modern_driver sometimes keeps executing one trajectory list behind #95

Closed
fjonath1 opened this issue Feb 28, 2017 · 9 comments
Closed

ur_modern_driver sometimes keeps executing one trajectory list behind #95

fjonath1 opened this issue Feb 28, 2017 · 9 comments

Comments

@fjonath1
Copy link

fjonath1 commented Feb 28, 2017 •
edited
Loading

Hi,

My research group at university has been using ur_modern_driver for awhile, and sometimes we noticed a strange bug where ur_modern_driver never finishes executing a trajectory (it just stopped halfway). Whenever this happened, after we cancel the goal, if we input a new trajectory list, it will be executing 1 trajectory list behind.

here is the log we have when it happened. Note: sec 24 in follow_joint_trajectory/result and sec 26 in /follow_joint_trajectory/goal is the last published msg/response we got before cancelling the goal.
Afterward, the follow_joint_trajectory/result always says error code: 0 and error_string: Goal canceled by client. The published joint state reflects the actual robot joint position.

Restarting the ur_modern_driver will temporarily fix this problem, until it resurfaces for unknown reason.

Any helps would be appreciated,
Felix

rostopic echo /follow_joint_trajectory/goal

---
header: 
  seq: 25
  stamp: 
    secs: 1488322862
    nsecs: 751332044
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1488322862
    nsecs: 751256942
  id: /simple_ur5_driver_node-25-1488322862.751
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: ''
    joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [0.30003762358163316, -1.3300827834477285, -1.7997786308312367, -0.2700036285261235, 1.4998983234390535, 1.6002492253792502]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        positions: [0.2740209366499169, -1.3557743539621965, -1.8373377133488311, -0.1987249852305596, 1.5041957780008162, 1.6021728750855273]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 169328642
      - 
        positions: [0.22599013000674842, -1.4032049456812143, -1.906677557996698, -0.06713364376182626, 1.5121295402686858, 1.6057242283894235]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 338657284
      - 
        positions: [0.16502795234426537, -1.463405312093814, -1.994685822357452, 0.09988613579464298, 1.5221993154548281, 1.6102317152751378]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 507985927
      - 
        positions: [0.10250264192120583, -1.525149277645198, -2.084950708881302, 0.2711884738012781, 1.5325272900047175, 1.6148547787476653]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 677314569
      - 
        positions: [0.04839420020894276, -1.5785815555262035, -2.16306455298848, 0.4194308816916354, 1.5414649602882757, 1.6188555067527373]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 846643212
      - 
        positions: [0.008437197098348496, -1.6180392376537152, -2.2207486224830117, 0.5289021982875916, 1.548065086036134, 1.6218098905103289]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs:  15971854
      - 
        positions: [-0.016663997163435074, -1.6428267559133058, -2.2569860507552177, 0.5976726407645384, 1.5522113188777373, 1.6236658495375338]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 185300497
      - 
        positions: [-0.02990418754327696, -1.6559014908194636, -2.2761002986350625, 0.6339471598732576, 1.5543983427941874, 1.624644816936499]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 354629139
      - 
        positions: [-0.03563311607301624, -1.6615588280384743, -2.284370886659995, 0.649642865256838, 1.5553446512195743, 1.6250684085995128]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 523957782
      - 
        positions: [-0.03759172411737154, -1.663492960421042, -2.2871984381215107, 0.6550089183794296, 1.5556681754675699, 1.6252132262620818]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 693286424
      - 
        positions: [-0.03808890923632328, -1.6639839324873862, -2.2879162011848186, 0.6563710703259337, 1.5557503008535996, 1.6252499876687339]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 862615067
      - 
        positions: [-0.03817235289265084, -1.6640663333936256, -2.2880366649157233, 0.6565996832400323, 1.555764084135171, 1.6252561574153048]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs:  31943709
      - 
        positions: [-0.0381790390830617, -1.6640729360303435, -2.2880463174582637, 0.656618001582508, 1.555765188564784, 1.6252566517860236]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 201272352
      - 
        positions: [-0.03817930138745474, -1.664073195056861, -2.288046696134933, 0.6566187202251744, 1.5557652318924073, 1.6252566711805672]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 370600994
      - 
        positions: [-0.03817930649111488, -1.6640732000967435, -2.2880467035028498, 0.6566187342078146, 1.5557652327354337, 1.625256671557927]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 539929637
      - 
        positions: [-0.038179306530678137, -1.6640732001358123, -2.2880467035599654, 0.6566187343162071, 1.5557652327419687, 1.6252566715608523]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 709258279
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---
header: 
  seq: 26
  stamp: 
    secs: 1488322865
    nsecs: 775063037
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1488322865
    nsecs: 775028944
  id: /simple_ur5_driver_node-26-1488322865.775
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: ''
    joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [-0.038390898830618525, -1.6642658158903219, -2.288329537497892, 0.6571696412718988, 1.5557814352918464, 1.6252728742097837]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        positions: [-0.03718284135872967, -1.6806558343009068, -2.290063694912922, 0.6379053192919489, 1.5433001111963, 1.6241308836145596]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 179052190
      - 
        positions: [-0.03495258141062718, -1.7109143298281408, -2.2932652162945155, 0.6023404171751184, 1.5202576667122145, 1.622022593284915]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 358104380
      - 
        positions: [-0.03212186686111248, -1.7493193433819378, -2.2973286857403847, 0.5572003491037567, 1.4910114871747215, 1.6193466863280583]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 537156570
      - 
        positions: [-0.029218569887251244, -1.7887091008730116, -2.3014963467105067, 0.5109028433895395, 1.4610154055978055, 1.6166021663723078]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 716208760
      - 
        positions: [-0.026706101352179025, -1.8227963910095177, -2.3051029763961894, 0.47083769421377464, 1.4350572580793206, 1.6142271010259854]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 895260950
      - 
        positions: [-0.024850739972433384, -1.847968543725707, -2.3077663337025394, 0.4412511225147483, 1.4158881645272086, 1.6124732066163934]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs:  74313140
      - 
        positions: [-0.02368519243900343, -1.8637818191499798, -2.309439468420631, 0.42266468644741123, 1.403846041654728, 1.6113714011539573]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 253365330
      - 
        positions: [-0.023070398135655763, -1.87212288750564, -2.310322001019185, 0.41286085203826645, 1.397494152667046, 1.6107902290419032]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 432417520
      - 
        positions: [-0.022804381369784178, -1.8757320036210698, -2.310703866085867, 0.4086188083035403, 1.3947457391627605, 1.610538760339572]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 611469710
      - 
        positions: [-0.022713435466922095, -1.876965889472499, -2.3108344182454164, 0.4071685369412408, 1.3938061106142867, 1.610452788133647]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 790521900
      - 
        positions: [-0.022690349199272493, -1.8772791066501695, -2.310867558408994, 0.40680039113388783, 1.3935675895212125, 1.6104309644198351]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 969574090
      - 
        positions: [-0.022686474580925705, -1.8773316745681001, -2.31087312039449, 0.40673860442496146, 1.3935275580090882, 1.610427301698636]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 148626280
      - 
        positions: [-0.02268616411471202, -1.8773358867410113, -2.310873566066405, 0.4067336535668744, 1.3935243503558732, 1.6104270082113603]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 327678470
      - 
        positions: [-0.022686151934883637, -1.8773360519877946, -2.310873583550457, 0.40673345934090327, 1.3935242245171702, 1.6104269966976288]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 506730660
      - 
        positions: [-0.02268615169790055, -1.8773360552030036, -2.3108735838906442, 0.4067334555618459, 1.3935242220687247, 1.610426996473606]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 685782851
      - 
        positions: [-0.022686151696063472, -1.8773360552279277, -2.3108735838932812, 0.4067334555325509, 1.3935242220497446, 1.6104269964718694]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 864835041
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---

rostopic echo /follow_joint_trajectory/goal

---
header: 
  seq: 23
  stamp: 
    secs: 1488322862
    nsecs: 255526896
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1488322860
      nsecs: 763487100
    id: /simple_ur5_driver_node-24-1488322860.763
  status: 3
  text: ''
result: 
  error_code: 0
  error_string: ''
---
header: 
  seq: 24
  stamp: 
    secs: 1488322865
    nsecs: 548539396
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1488322862
      nsecs: 751256942
    id: /simple_ur5_driver_node-25-1488322862.751
  status: 3
  text: ''
result: 
  error_code: 0
  error_string: ''
---
header: 
  seq: 25
  stamp: 
    secs: 1488323099
    nsecs: 192089845
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1488322865
      nsecs: 775028944
    id: /simple_ur5_driver_node-26-1488322865.775
  status: 2
  text: ''
result: 
  error_code: -100
  error_string: Goal cancelled by client
---
header: 
  seq: 26
  stamp: 
    secs: 1488323102
    nsecs: 243607138
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1488323099
      nsecs: 296827077
    id: /simple_ur5_driver_node-27-1488323099.297
  status: 3
  text: ''
result: 
  error_code: 0
  error_string: Goal cancelled by client
---
---
header: 
  seq: 27
  stamp: 
    secs: 1488323110
    nsecs: 676625450
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1488323109
      nsecs: 282537937
    id: /simple_ur5_driver_node-28-1488323109.283
  status: 3
  text: ''
result: 
  error_code: 0
  error_string: Goal cancelled by client
---

The last /joint_states when the robot never finishes/execute the trajectory we sent.

header: 
  seq: 93739
  stamp: 
    secs: 1488322875
    nsecs: 735777683
  frame_id: ''
name: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
position: [-0.038390898830618525, -1.66428467107084, -2.28834839267841, 0.6571372359277533, 1.5557814352918464, 1.6252728742097837]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: [-0.2981608185299103, 1.7777558578512693, 1.4616605539962517, 0.9348502409565058, 0.19215518819008112, 0.4468370646007442]
---
@xqms
Copy link

xqms commented Mar 24, 2017

This may be related to my PR #101 which addresses some race conditions present in the trajectory action goal/cancel callbacks. Maybe you can check if you can still reproduce the problem with #101?

I also noticed that the error string is never reset, so subsequent action calls after a failure will always return the old error string.

@fjonath1
Copy link
Author

This may be related to my PR #101 which addresses some race conditions present in the trajectory action goal/cancel callbacks. Maybe you can check if you can still reproduce the problem with #101?

Interesting. I do noticed that this problems seems to happen more often with more stop trajectory in the middle of execution, although it usually happened randomly. That might be in fact related to the issue you addressed on your PR.

I will try #101 and get back to you. It might take few days to ascertain whether it fixes this problem or not, given the randomity behavior of this bug.

@a-price
Copy link

a-price commented Jul 25, 2017

I'm seeing this behavior on 9ca8836. I'll try #101 as well and report back.

@a-price
Copy link

a-price commented Jul 25, 2017

I haven't seen the retained trajectory bug on #101, but the server appears to be deadlocking after running for a period of time.

Output from the follow_joint_trajectory/status channel:

---
header: 
  seq: 2614
  stamp: 
    secs: 1500952834
    nsecs: 250381600
  frame_id: ''
status_list: 
  - 
    goal_id: 
      stamp: 
        secs: 1500952750
        nsecs: 823723015
      id: /capture_experiment-96-1500952750.823723015
    status: 2
    text: ''
  - 
    goal_id: 
      stamp: 
        secs: 1500952781
        nsecs:  77816914
      id: /capture_experiment-97-1500952781.77816914
    status: 7
    text: ''

Output from node:

[ INFO] [1500952750.868821747]: on_goal
[DEBUG] [1500952750.869189614]: Accepting goal, id: /capture_experiment-96-1500952750.823723015, stamp: 1500952750.82
[DEBUG] [1500952763.989070505]: The action server has received a new cancel request
[DEBUG] [1500952763.989228775]: Transisitoning to a cancel requested state on goal id: /capture_experiment-96-1500952750.823723015, stamp: 1500952750.82
[ INFO] [1500952763.989467065]: on_cancel
[DEBUG] [1500952763.989660130]: Setting status to canceled on goal, id: /capture_experiment-96-1500952750.823723015, stamp: 1500952750.82
[DEBUG] [1500952763.989778564]: Publishing result for goal with id: /capture_experiment-96-1500952750.823723015 and stamp: 1500952750.82
[DEBUG] [1500952781.141009364]: The action server has received a new goal request
[ INFO] [1500952781.141109694]: on_goal
[DEBUG] [1500952812.011810600]: The action server has received a new cancel request
[DEBUG] [1500952812.011858699]: Transisitoning to a cancel requested state on goal id: /capture_experiment-96-1500952750.823723015, stamp: 1500952750.82
[DEBUG] [1500952812.011892001]: Transisitoning to a cancel requested state on goal id: /capture_experiment-97-1500952781.77816914, stamp: 1500952781.08

Action 96 did not execute anything (maybe the deadlock starts here?), so the client timed out and sent a cancel request. Notice that the debug statements say it's trying to cancel the 96 goal twice. After this point, the node is completely locked, and sending additional goals or cancels has no effect and produces no debug statements.

This behavior has been observed multiple times over the past hour or so.

@danielkruse
Copy link

I am also observing the retained trajectory bug issue. Ours seems to be triggered infrequently by a trajectory cancellation. Has #101 demonstrated an appropriate fix?

@smurray47
Copy link

I can't guarantee correctness, but we were having the same problem, and #101 has fixed it, or at least made it much more infrequent. I can also confirm we also experience very occasional deadlock with that pull request, but that is a MUCH more preferable mode of failure.

This is a dangerous bug! I'm surprised that pull request has been sitting for a year. Our UR5 would freak out every once in a while when we cancelled and then gave a new trajectory.

@prajval10
Copy link

I am also facing the same issue. If I lower the velocity slider bar in the UR teach pendant and run the trajectory, the robot goes into an abrupt protective stop. After which, I enable the robot from the pendant and cancel the goals and send a new trajectory. This time the robot goes into various jerky random motions before going to desired pose. This must be due to the retained trajectory. Has anyone found a proper fix to the problem? I will check #101.

@gavanderhoorn
Copy link
Member

I'm closing this as we've officially deprecated this package.

Refer to the announcement on ROS Discourse.

@gavanderhoorn
Copy link
Member

ur_robot_driver makes use of ros_control's JointTrajectoryController, which should not exhibit this behaviour.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
7 participants
@xqms @a-price @danielkruse @gavanderhoorn @smurray47 @fjonath1 @prajval10