Failure due to protective stop on the UR

[ros-industrial]

Failure due to protective stop on the UR

[VictorLamoine]

General information :

• ROS Indigo, Ubuntu 14.04 64 bits.
• The end effector mass is 0.5 kg and it is declared in the UR10 controller.
• I'm using universal_robot indigo-branch.

These are my test pacakges:
https://github.com/InstitutMaupertuis/institut_maupertuis_robots_descriptions/tree/ur10_tests
https://github.com/InstitutMaupertuis/universal_robot_motion
https://github.com/InstitutMaupertuis/ensenso_extrinsic_calibration/tree/ur10_test

kinematics.yaml file
joint_limits.yaml file

universal_robot_motion

This program moves the robot using the tool0 TCP (manipulator move group).
The robot does not go into protective stop with this trajectory/setup however the robot motion is very jerky at high speeds on the teach pendant.

• Video 1, speed = 24%
• Video 1, speed = 100%

$ rostopic echo -n1 /joint_states
header: 
  seq: 30115
  stamp: 
    secs: 1465464907
    nsecs: 757682323
  frame_id: ''
name: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
position: [0.10749213397502899, -1.6914003531085413, -2.255641285573141, -0.7656462828265589, 1.570691466331482, -1.4633196035968226]
velocity: [0.003847735235467553, 0.0005752427969127893, -0.0010865697404369712, -0.017736652866005898, 0.0, 0.00046019424917176366]
effort: [0.3799438774585724, 3.437805414199829, 2.176798105239868, 1.029556393623352, -0.1547699123620987, 0.1682281643152237]
---

$ rostopic echo /joint_trajectory_action/goal
header: 
  seq: 27
  stamp: 
    secs: 1465464915
    nsecs: 547241536
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1465464915
    nsecs: 547242262
  id: /move_group-28-1465464915.547242262
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /base_link
    joint_names: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [-2.2549708525287073, -1.6918318907367151, 0.10376204550266266, -0.7655866781817835, 1.5708353519439697, -1.467086140309469]
        velocities: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 200000000
      - 
        positions: [-2.2551142205139545, -1.6917144345912636, 0.104558172139388, -0.7655606542365021, 1.5708275466449029, -1.4662796425854197]
        velocities: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 400000000
      - 
        positions: [-2.2552569478965943, -1.6915974960830145, 0.10535434386167308, -0.7655347639600301, 1.570819741379175, -1.4654730997769168]
        velocities: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 600000000
      - 


.............

      - 
        positions: [-2.255681287524319, -1.6912498011257657, 0.10774311980599016, -0.7654578901431304, 1.5707963248173944, -1.4630532105786997]
        velocities: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 8
          nsecs:         0
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---

ensenso_extrinsic_calibration

This program moves the robot using the ensenso_tcp TCP (ensenso_n10 move group), I also tried with manipulator, same results.
The robot goes into protective stop. At low speeds (~ 20%) the robot either goes into protective stop or does not finish it's trajectory.

Video, speed = 24% is a video of the robot going in protective stop.

roslaunch ur10_ensenso_extrinsic_calibration calibration.launch robot_ip:=192.168.1.10 sim:=false

$ rostopic echo -n1 /joint_states
header: 
  seq: 278
  stamp: 
    secs: 1465465642
    nsecs: 148979714
  frame_id: ''
name: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
position: [0.07957300543785095, -1.9691742102252405, -1.880716625844137, -0.8625786940204065, 1.5707874298095703, -1.4912903944598597]
velocity: [0.0, 0.008897088468074799, -0.0, 0.019481556490063667, 0.023815052583813667, 0.01781335286796093]
effort: [-0.0274658240377903, 4.591370105743408, 3.295461416244507, -0.0695343092083931, -0.1211242824792862, -0.1256103664636612]
---

$ rostopic echo /joint_trajectory_action/goal
dell@M6700:~/catkin_src/ensenso_extrinsic_calibration$ rostopic echo /joint_trajectory_action/goal
header: 
  seq: 0
  stamp: 
    secs: 1465465684
    nsecs: 729480224
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1465465684
    nsecs: 729483202
  id: /move_group-1-1465465684.729483202
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /base_link
    joint_names: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [-1.880716625844137, -1.969210449849264, 0.0795610249042511, -0.8626983801471155, 1.5707154273986816, -1.491206471120016]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 200000000
      - 
        positions: [-1.7018506974226018, -2.0803188339097662, -0.01981388490404612, -0.9063977531316931, 1.528335916287313, -2.969048055500995]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 400000000
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---
header: 
  seq: 1
  stamp: 
    secs: 1465465687
    nsecs: 247310453
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1465465687
    nsecs: 247311127
  id: /move_group-2-1465465687.247311127
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /base_link
    joint_names: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [-1.8746102491961878, -1.9737761656390589, 0.07535193860530853, -0.8645313421832483, 1.5689294338226318, -1.54619008699526]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 200000000
      - 
        positions: [-1.8807166263011048, -1.9692104516566022, 0.07956102492095352, -0.8626983796833887, 1.5707154272559791, -1.4912064711034585]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 400000000
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---
header: 
  seq: 2
  stamp: 
    secs: 1465465698
    nsecs: 171089184
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1465465698
    nsecs: 171090400
  id: /move_group-3-1465465698.171090400
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /base_link
    joint_names: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [-1.874622646962301, -1.9738004843341272, 0.07539987564086914, -0.8645313421832483, 1.5689294338226318, -1.5461538473712366]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 200000000
      - 
        positions: [-1.7832820861832914, -2.0082614073819967, 0.0781178191017029, -1.0115700827663803, 1.4446979420760742, -2.1014648702613377]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 400000000
      - 
        positions: [-1.6651512276537161, -2.073190960918998, 0.10424251270339324, -1.082737436077796, 1.294429050315901, -2.6361261810050793]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 600000000
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---
header: 
  seq: 3
  stamp: 
    secs: 1465465700
    nsecs: 856730864
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1465465700
    nsecs: 856731847
  id: /move_group-4-1465465700.856731847
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /base_link
    joint_names: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [-1.8433497587787073, -1.9871748129474085, 0.07688666880130768, -0.9108617941485804, 1.5257060527801514, -1.7314875761615198]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 200000000
      - 
        positions: [-1.8746345683462238, -1.9737642465191048, 0.07541186363020014, -0.8645194207857712, 1.568929433778452, -1.5461781660645038]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 400000000
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---
header: 
  seq: 4
  stamp: 
    secs: 1465465705
    nsecs: 216729253
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1465465705
    nsecs: 216729984
  id: /move_group-5-1465465705.216729984
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /base_link
    joint_names: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [-1.8507488409625452, -1.9846580664264124, 0.07609570771455765, -0.9010370413409632, 1.5348515510559082, -1.6904967466937464]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 200000000
      - 
        positions: [-1.4816874711486596, -2.1910048161415308, 0.008885546546005152, -1.1846888096274952, 1.4944890624148757, -3.107264129418932]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 400000000
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---
header: 
  seq: 5
  stamp: 
    secs: 1465465707
    nsecs: 734270208
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1465465707
    nsecs: 734271261
  id: /move_group-6-1465465707.734271261
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /base_link
    joint_names: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [-1.8480427900897425, -1.9862878958331507, 0.0754837691783905, -0.9033015410052698, 1.5345759391784668, -1.7015331427203577]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 200000000
      - 
        positions: [-1.8507369204866926, -1.9846223054378545, 0.0760957076899158, -0.9010131990373642, 1.5349473955385382, -1.6905444304981287]
        velocities: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 400000000
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---

[ThomasTimm]

From the looks of it, my initial guess would be that your problem is due to the velocity values in your goal.
In the first example, each goal pose has a velocity of 0.01, spaced 0.2 sec apart. Obviously, this will result in jerky movement, as the robot first accelerates to be able to move sufficiently fast, and then slows down to get the velocity down to 0.01 rad/sec. The result is the jerky motion you see.
The same goes with the other example; here the velocity is a bit higher, but the travel distance is also much higher.
Another issue (and what looks like to be the main problem in example 2) is the fact that you don't include a final pose where all velocities are 0. You are trying to the robot to go from a velocity of 0.05 rad/sec to a velocity of 0 instantaneously. You should also have a start pose with time_from_start=0 and the joint positions at the same values as the current state to comply the standards, but this is not strictly required.

To visualize the problem, try to plot the velocity and acceleration curve required to execute your trajectory, interpolating with a timestep of 8 ms. You can find the code used by the driver for interpolation at https://github.com/ThomasTimm/ur_modern_driver/blob/master/src/ur_driver.cpp#L61

[gavanderhoorn]

@VictorLamoine: what did you use to calculate velocities and accelerations?

[VictorLamoine]

Argh, I didn't notice that the trajectory already had timing information!
I never played with timed/velocity constrained trajectories before.

Some things are not clear to me:

• How does it work when we specify both the goal time and velocity, these information are contradictory?!
• What should I put in velocities?
  • Per-axis maximum/goal velocities?
  • TCP maximum/goal velocity?
• If I want a specific TCP speed, what should I do?
• Can I put all time_from_start to zero If I want to ignore this information?

Where is that documented? I can't find the right wiki link.

[ThomasTimm]

The velocity is related to the point at the trajectory, so you should use the desired joint velocity at the point (i.e. all zeros at the first and last point).
The driver then interpolates and calculate the required velocity to reach the trajectory points at the desired time.
So goal time and velocity is not contradictory; goal time is when the robot should be at that pose, velocity is how fast the robot should move at that point.
If you want a specific TCP speed in cartesian space, you should find and calculate the inverse Jacobian.
No, if you put all time_from_start to zero, you are essentially telling the driver at should be at all those poses at the same time (now).

[VictorLamoine]

Thanks, I'll try that Thursday and give you feedback!

[VictorLamoine]

I think understood how it works but this feels completely un-usable to me.

All I want is a smooth motion at a given constant speed. I don't care about accelerations as long as it's not too slow. And I don't want to calculate anything, ROS should do that for me.

There are many parameters interacting with each other:

• Compute Cartesian path eef_step
• time_from_start (can we change how much it increases between each point? I don't want to manually tweak those values!)
• Velocities

If we change one of those without adjusting the others, it breaks everything.

It's hard to believe people are doing it this way.. this is way over-complicated for what I'm trying to achieve, am I using the right tool? Is there a package or anything I'm missing?

I'm frustrated because I'm trying to achieve something that should be very simple to achieve (10 secs of programming with the UR teach pendant) and it feels like a pain to do it with ROS-I.

I'm getting a lot less preventing stops because I understood how to tweak the parameters but it still happens sometimes. The robot is finishing the trajectory very brutally despite the fact that I set all the velocities of the last pose to zero, it seems like it stops before reaching the goal.

[ThomasTimm]

Seeing your issue is with ROS and calculating stuff, I'll close the topic. Let me know if you have any issues with the driver not working when using it as intended.