driver only executing a portion of the planned trajectory #43_[error] [1542697186.806706372]: goal start doesn't-CSDN博客

I am operating ur5 with firmware 3.2.18744. I'm sending planning requests to move_group with either moveit_commander or rviz. The planning to the goal state is performed without problem, as far as I can tell from rviz and the move_group console output. However during execution the robot only performs a small portion of the planned trajectory.

There is no error message associated with pre-mature ending of motion and the driver console outputs "on_goal". I am then able to keep the same target position in rviz, making no changes to the set up and then I plan and execute again which again results in the robot performing the next small increment of the planed trajectory.

Just a side note; I'm also receiving the "Wrong length of message on RT interface: 1071797593" message intermittently.

nick-pestell opened this issue on 22 Apr 2016 · 8 comments

ThomasTimm commented on 26 Apr 2016

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 opened this issue on 22 Apr 2016 · 8 comments

nick-pestell commented on 27 Apr 2016

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

nick-pestell opened this issue on 22 Apr 2016 · 8 comments

ThomasTimm commented on 27 Apr 2016

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 opened this issue on 22 Apr 2016 · 8 comments

nick-pestell commented on 27 Apr 2016

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 opened this issue on 22 Apr 2016 · 8 comments

nick-pestell commented on 28 Apr 2016

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.

Owner

ThomasTimm commented on 3 May 2016

Glad to hear you got it to work!

ThomasTimm closed this on 3 May 2016

nick-pestell referenced this issue on 10 Jun 2016

Open

Support for UR firmware 3.2 #17

YMmirsky commented on 30 Jun 2016

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 commented on 13 Jul 2016

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