driver only executing a portion of the planned trajectory

[ros-industrial]

driver only executing a portion of the planned trajectory

[ThomasTimm]

That sounds a bit strange and without more information I can't help you out.

Try to have a look at what is published to the topics /follow_joint_trajectory/goal and /follow_joint_trajectory/result and see if you can figure it out.

If not, please post the data published to /follow_joint_trajectory/goal here, and I'll try to see if I can find the problem.
The "Wrong length"-message can be ignored.

[nick-pestell]

Thanks for your quick response. I have tested using a a modified test_move.py which doesn't have the premature stopping, although the robot does seem to exhibit a fairly abrupt jolt during motions lasting 10 seconds or more. I suppose this suggests my issue related to moveit rather than the driver?

With test_move.py follow_joint_trajectory/goal has the following:

header: 
  seq: 1
  stamp: 
    secs: 1461692309
    nsecs: 635520935
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1461692309
    nsecs: 635494947
  id: /ur5_client-1-1461692309.635
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.3321382999420166, -0.40130836168398076, -1.332825009022848, -2.35036546388735, 0.9054464101791382, -0.5632146040545862]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 0
      - 
        positions: [0.0, -1.5707963267948966, 0.0, -1.5707963267948966, 0.0, 0.0]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 10
          nsecs: 0
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs: 0
---

These two messages are concurrent with my code I believe. On follow_joint_trajectory/result:

header: 
  seq: 16
  stamp: 
    secs: 1461693171
    nsecs: 158085380
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1461693161
      nsecs: 48934936
    id: /ur5_client-1-1461693161.049
  status: 3
  text: ''
result: 
  error_code: 0
  error_string: Goal start doesn't match current pose

When using moveit follow_joint_trajectory/goal is displaying continuously updated joint positions at roughly 4 per second. Right up until the premature ending when the output from the driver console is "on_goal". On follow_joint_trajectory/result I have:

header: 
  seq: 14
  stamp: 
    secs: 1461693012
    nsecs: 508863791
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1461693010
      nsecs: 787766573
    id: /move_group-2-1461693010.787766573
  status: 3
  text: ''
result: 
  error_code: 0
  error_string: Goal start doesn't match current pose

[ThomasTimm]

Well then, there is your answer; the start pose doesn't match the current pose of the robot. The driver then discards the trajectory for safety reasons. Have a look at #35 and #36 for more information.

The thing I don't get is why MoveIt continuously publishes the trajectory at 4 Hz. It should just calculate the trajectory and then send it once since, as you say, the target pose doesn't change. If you can get MoveIt/rviz to only publish the trajectory once, that would solve your problem. Otherwise increase the tolerance in https://github.com/ThomasTimm/ur_modern_driver/blob/master/src/ur_ros_wrapper.cpp#L333 from 0.01 to i.e. 0.1.
Or, if you can wait about a month and a half, I should have made it so you can set the tolerance as a parameter if you don't feel comfortable changing the source code.

[nick-pestell]

Thanks again for your help.

Just as an up-date; slightly irritatingly I've been unable to reproduce the two messages on follow_joint_trajectory/result and am now getting;

  seq: 7
  stamp: 
    secs: 1461748769
    nsecs: 52494522
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1461748748
      nsecs: 959388017
    id: /ur5_client-1-1461748748.959
  status: 3
  text: ''
result: 
  error_code: 0
  error_string: ''
---

and pretty much the same for the moveit version.

I'm still experiencing the premature ending with the tolerance increased, which makes me think that perhaps it's got less to do with the start pose not matching current pose and more to do with moveit publishing multiple trajectories instead of one goal. I'm going to look into this and in the mean time try using the moveit-get position IK service.

Thanks again!

[nick-pestell]

I've now got it working ok. I think it was to do with a moveit_commander function I was using, execute(), which doesn't transfer from simulation to the real robot so well. Instead I'm using publishing straight on the follow_joint_trajectory/action topic which is working great. Thank you for your help and thanks for writing such a great driver.

[ThomasTimm]

Glad to hear you got it to work!

[YMmirsky]

Hi, I'm trying to get my UR5 moving with rviz and I'm having the exact same problem as you did. Can you tell me what you did to solve it? Thanks.

[nick-pestell]

Hi Yosef,

I'm sorry for the late reply! There's two ways in which you can try to fix this issue. Firstly lower all the joint velocity max limits in /ur5_moveit_config/config/joint_limits.yaml. I have all mine set to around 0.2. If you are using the moveit_commander (i.e. with plan & execute() )or equivalent c++ API, this should solve it. I'm not sure why this worked for me but it did....

Another way around this is to publish joint states straight /follow_joint_trajectory/goal topic in your code. You can use the GetPositionIK action service to get a set of joint values from a set. Cartesian and quaternion coordinates. I seem to be finding this method the best way of talking with the robot for simple point to point movements at the moment.

If your not sure about using ros services the tutorials are not to bad on this,

http://wiki.ros.org/ROS/Tutorials/CreatingMsgAndSrv