ros-industrial/ur_modern_driver

(deprecated) ROS 1 driver for CB1 and CB2 controllers with UR5 or UR10 robots from Universal Robots

ur_modern_driver - Deprecated

Deprecation notice

note: the master branch contains a deprecated version of this driver, kept for archival purposes only.

ur_modern_driver has been deprecated completely in favour of ur_robot_driver.

Users with CB3 and e-Series controllers should use ur_robot_driver.

Refer to the announcement on ROS Discourse for more information.

Click to see original readme

Overview

A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is designed to replace the old driver transparently, while solving some issues, improving usability as well as enabling compatibility of ros_control.

Improvements

• A script is only running on the robot while a trajectory is actually executing. This means that the teach pendant can be used to move the robot around while the driver is connected.

• The driver exposes the same functionality as the previous ur_driver:

  • Action interface on /follow_joint_trajectory for seamless integration with MoveIt

  • Publishes robot joint state on /joint_states

  • Publishes TCP force on /wrench

  • Publishes IO state on /ur_driver/io_states (Note that the string /ur_driver has been prepended compared to the old driver)

  • Service call to set outputs and payload - Again, the string /ur_driver has been prepended compared to the old driver (Note: I am not sure if setting the payload actually works, as the robot GUI does not update. This is also true for the old ur_driver )

• Besides this, the driver subscribes to two new topics:

  • /ur_driver/URScript : Takes messages of type std_msgs/String and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like.

  • /joint_speed : Takes messages of type trajectory_msgs/JointTrajectory. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint_names, so set the values in the correct order.

• Added support for ros_control.

  • As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time.
  • With ros_control active, the driver doesn't open the action_lib interface nor publish joint_states or wrench msgs. This is handled by ros_control instead.
  • Currently two controllers are available, both controlling the joint position of the robot, useable for trajectroy execution
    • The velocity based controller sends joint speed commands to the robot, using the speedj command
    • The position based controller sends joint position commands to the robot, using the servoj command
    • I have so far only used the velocity based controller, but which one is optimal depends on the application.
  • As ros_control continuesly controls the robot, using the teach pendant while a controller is running will cause the controller on the robot to crash, as it obviously can't handle conflicting control input from two sources. Thus be sure to stop the running controller before moving the robot via the teach pendant:
    • A list of the loaded and running controllers can be found by a call to the controller_manager rosservice call /controller_manager/list_controllers {}
    • The running position trajectory controller can be stopped with a call to rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'pos_based_pos_traj_controller' strictness: 1" (Remember you can use tab-completion for this)

Installation

As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.

Just clone the repository into your catkin working directory and make it with catkin_make.

Note that this package depends on ur_msgs, hardware_interface, and controller_manager so it cannot directly be used with ROS versions prior to hydro.

Usage

The driver is designed to be a drop-in replacement of the ur_driver package. It won't overwrite your current driver though, so you can use and test this package without risking to break your current setup.

If you want to test it in your current setup, just use the modified launch files included in this package instead of those in ur_bringup. Everything else should work as usual.

If you would like to run this package to connect to the hardware, you only need to run the following launch file.

roslaunch ur_modern_driver urXX_bringup.launch robot_ip:=ROBOT_IP_ADDRESS

Where ROBOT_IP_ADDRESS is your UR arm's IP and XX is '5' or '10' depending on your robot. The above launch file makes calls to both roscore and the launch file to the urXX_description so that ROS's parameter server has information on your robot arm. If you do not have your ur_description installed please do so via:

sudo apt install ros-<distro>-ur-description

Where is the ROS distribution your machine is running on. You may want to run MoveIt to plan and execute actions on the arm. You can do so by simply entering the following commands after launching ur_modern_driver:

roslaunch urXX_moveit_config ur5_moveit_planning_executing.launch
roslaunch urXX_moveit_config moveit_rviz.launch config:=true

---

If you would like to use the ros_control-based approach, use the launch files urXX_ros_control.launch, where XX is '5' or '10' depending on your robot.

Note: If you are using the ros_control-based approach you will need 2 packages that can be found in the ur_driver package. If you do not have the ur_driver package in your workspace simply copy these packages into your workspace /src folder:

• urXX_moveit_config
• ur_description

The driver currently supports two position trajectory controllers; a position based and a velocity based. They are both loaded via the launch file, but only one of them can be running at the same time. By default the velocity based controller is started. You can switch controller by calling the appropriate service:

rosservice call /universal_robot/controller_manager/switch_controller "start_controllers:
- 'vel_based_pos_traj_controller'
stop_controllers:
- 'pos_based_pos_traj_controller'
strictness: 1"

Be sure to stop the currently running controller either before or in the same call as you start a new one, otherwise it will fail.

The position based controller should stay closer to the commanded path, while the velocity based react faster (trajectory execution start within 50-70 ms, while it is in the 150-180ms range for the position_based. Usage without ros_control as well as the old driver is also in the 170ms range, as mentioned at my lightning talk @ ROSCon 2013).

Note that the PID values are not optimally tweaked as of this moment.

To use ros_control together with MoveIt, be sure to add the desired controller to the controllers.yaml in the urXX_moveit_config/config folder. Add the following

controller_list:
 - name: /vel_based_pos_traj_controller #or /pos_based_pos_traj_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint

Using the tool0_controller frame

Each robot from UR is calibrated individually, so there is a small error (in the order of millimeters) between the end-effector reported by the URDF models in https://github.com/ros-industrial/universal_robot/tree/indigo-devel/ur_description and the end-effector as reported by the controller itself.

This driver broadcasts a transformation between the base link and the end-effector as reported by the UR. The default frame names are: base and tool0_controller.

To use the tool0_controller frame in a URDF, there needs to be a link with that name connected to base. For example:

<!-- Connect tool0_controller to base using floating joint -->
<link name="tool0_controller"/>
<joint name="base-tool0_controller_floating_joint" type="floating">
  <origin xyz="0 0 0" rpy="0 0 0"/>
  <parent link=base"/>
  <child link="tool0_controller"/>
</joint>

Now, the actual transform between base and tool0_controller will not be published by the robot_state_publisher but will be taken from this driver via /tf.

NOTE: You need an up-to-date version of robot_state_publisher that is able to deal with floating joints, see: ros/robot_state_publisher#32

Compatibility

Should be compatible with all robots and control boxes with the newest firmware.

###Tested with:

*Real UR10 with CB2 running 1.8.14035

*Real UR5 with CB2 running 1.8.14035

*Simulated UR3 running 3.1.18024

*Simulated UR5 running 3.0.16471

*Simulated UR5 running 1.8.16941

*Simulated UR5 running 1.7.10857

*Simulated UR5 running 1.6.08725

#Credits Please cite the following report if using this driver

@techreport{andersen2015optimizing, title = {Optimizing the Universal Robots ROS driver.}, institution = {Technical University of Denmark, Department of Electrical Engineering}, author = {Andersen, Thomas Timm}, year = {2015}, url = {http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html} }

The report can be downloaded from http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html

ur_modern_driver's Issues

joint name prefix has no effect, chrases driver

From Ilia Baranov at Clearpath Robotics:

Here is something intersting, setting param

`

`

seems to have no effect.

However, manual changing the joint_names in ur_ros_wrapper seems to crash the driver completely.

Wrong length of message on RT interface

From time to time (if the robot is running for multiple hours) I see log messages like:

Wrong length of message on RT interface: 1072623616
Wrong length of message on RT interface: 1072473427
Wrong length of message on RT interface: 1072398338
Wrong length of message on RT interface: 1072323238
Wrong length of message on RT interface: 1072473427
Wrong length of message on RT interface: 1072097960
Wrong length of message on RT interface: 0

Is this something to worry about?

I had a look at ur_communication.cpp:

select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
            bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes

Could it be sometimes more than one message is available, e.g. total data exceeding 2048 bytes and then with the next call a message is read in the middle? Maybe for unknown reasons the UR sends a burst or the thread context switching on the local machine was too slow?

Detect connection issues with doTraj

urDriver::openServo and urDriver::uploadProg should return true or false depending on whether the controller established a connection back to the driver to catch any problems, before urDriver::doTraj attemps to interpolate and send commands.
A connected_ flag should be updated accordingly and monitored in urDriver::servoj and in the loop of urDriver::doTraj

Feature request: Expose tool pose from RT connection in ROS

The standard URDF available in ROS Industrial (https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_description/urdf/ur5.urdf.xacro) does not give a perfect transform between base_link and ee_link, because each UR is calibrated individually and the URDF assumes nominal values for link dimensions, etc. This leads to an error in the order of [mm].

Extracting the calibrated DH parameters from a UR robot and use that to build a more accurate URDF model is possible but rather cumbersome and still does not return an exact solution (mismatch is about 0.1 mm in our experiments).

I think it would be possible to expose the tool pose that UR sends over the RT connection in ROS either as a geomtry_msg/PoseStamped topic or directly via /tf. That way, the URDF model can still be used for collision checking and visualization, but the more accurate tool pose can be used when calculating pick positions, etc.

Is there interest to expand the ur_modern_driver with this?

Feature request: Lock teach pendant when under ros_control

With the dashboard server, it is possible to lock the teach pendant from firmware version 3.1.17something by setting the user role to 'restricted' (http://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/dashboard-server-port-29999-15690/)
This can prevent the user from trying to use the screen and accidently crash the controller. ros_control is always controlling the robot when the controller is running, so moving the robot with the teach pendant will cause that input to struggle against ros_control, which doesn’t get the new target pose. The resulting conflicting motion will cause the controller to crash.
The enhancement should also display a message on the teach pendant to let them know that they are locked out.
The enhancement could be extended to be active whenever the driver is executing something on the robot (whether under ros_control or action interface). This would probably improve the stability of the driver.
Nevertheless it is quite important that the driver always unlocks the screen when the driver shuts down! Perhaps opening a new connection to the dashboard server, unlocking everything when the driver is killed, is the best way to go.

As I haven't tried it out, I am uncertain on how this works. The documentation says that it prevents the user from pressing the 'move' tab. If this is indeed the case and it is thus not possible to read the joint states and end effector position from the screen, then this enhancement should be made dependent on a parameter in the driver, allowing the user to turn it off.

Can this be bloomed, or can I bloom it?

use fixed time for servoj

Instead of transmitting the time in every servoj package, it should be a constant in the code uploaded in urDriver::uploadProg()

Evaluate max_velocity parameter for velocity / ros_control interfaces.

I am currently analyzing why a low-percentage of trajectories generated by MoveIt! trigger dangerous maximum speed movements of our UR5 to invalid positions. Most of the time the UR itself stops within the first second with the error "Protective Stop: Position deviates from path.". I personally had some shocking moments when without advance notice the robot turns to maximum speed (with an acceleration beyond all I have ever observed under "normal conditions").

I am analyzing three parts now which are involved: MoveIt!, the ros_controllers position-controller and the UR-Modern-Driver. I wrote a debugging-tool that analyzes the trajectory goal message as they are generated by MoveIt! and currently believe that at least part of the problem is MoveIt! setting invalid (e.g. old) states as start-state for a plan (e.g. observed by others in moveit/moveit_ros#442).

My question/suggestion regarding the UR-Modern-Driver is: Would it be possible to install a line of defense against invalid movements by adding a sanity check before sending servoj-commands to the UR? E.g. if the desired joint position are absolutely our of reach within the next 2-3 time-steps I think it would be reasonable to refuse the execution. This would be a great step towards a safe ROS-controlled UR5 (currently it really is life threatening if errors occur that I regularly saw over the last days...).

I could prepare a PR which adds the sanity check...

My testing environment: Ubuntu 14.04, ROS Indigo, UR5 CB3

Expose digital IO from realtime interface

At the moment, the IO states of the robot are communicated over a 'slow' interface of 10 Hz.
The real-time interface of UR (port 30003) communicates at least the 8 digital input pins at 125 Hz as well. From firmware 3.2 onward, the 8 digital output pins are also available on this interface.

I'd like to expose this data as a separate ROS message (e.g. /io_states_rt) with the same message type as /io_states (i.e. ur_msgs/IOStates.msg). This seems neater than increasing the publishing frequency of /io_states, since not all IO states are available through the real-time interface, like the analog and tool pins.

Btw: this would involve extending the real time interface data stream for firmware 3.2, as mentioned in #17, so I'd like to do that at the same time.

Suggestions welcome.

Failure due to protective stop on the UR

The robot stops during the execution of motions programmed via MoveIt.

Also read:
ros-industrial/universal_robot#241 (comment)
https://groups.google.com/forum/#!topic/swri-ros-pkg-dev/LwcFK44vmKI

The driver does not notice if the robot is halted due to E-stop or safety stop

This should be detected from the status broadcasted on 30002 in the robot_mode_data block (pkg type = 0)

Feature request: Implement ros_control velocity_based velocity_controller

[question] About the robot state publisher in launch files...

Is there any reason why the type="state_publisher" instead of type="robot_state_publisher" is being used here?

I'm on indigo, and I get this warning:
[ WARN] [1453385917.020716971]: The 'state_publisher' executable is deprecated. Please use 'robot_state_publisher' instead

Action interface sanity check doesn't actually confirm that trajectory start and current postion agree

The comments (and technical report) allude to the driver checking new trajectories for sane values however it is currently possible to send trajectories in which the trajectory's start does not agree with the robot's current position.

The issue seems to be with the if check in line 332 of ur_ros_wrapper.cpp

This can be replicated using the URSim with the driver along with RViz MoveIt! plugin with the following steps:

1. launch simulator, ur_modern_driver, and Rviz/MoveIt!
2. drag the goal some significant distance from the initial state and click Plan
3. confirm the plan is valid and then click Execute (Robot executes plan as expected)
4. without adjusting the goal state click Plan again (generates plan from original/initial state)
5. click Execute (plan is sent despite differing start positions, resulting in a protective stop in URSim)

-- Note: using Execute and Plan seems to use current robot position as the start state instead of the previously set start state via the Query GUI.

Feature request: rate limit publishing of topics

Hello,

Most of the topics this driver publishes are fairly fast (100hz or more). In most cases this is great, however if you are working on a bandwidth limited system, this quickly swamps transfer.

Setting a param for publishing rate would be great, not high priority.

joint_states.names empty when prefix parameter not found

Looking at ur_ros_wrapper.cpp#L93-L108, it seems the first if statement is only taken when the prefix parameter is found, so when this parameter is not set, no joint names are pushed into the joint_names vector.

This is probably caused by the false returned by ros::param::get in case the parameter is not there.

Make reverse_port settable from the parameter server

Overshoot with position-based ros_control interface (servoj)

I've had similar experience to what @andreaskoepf describes in #46. Working with the ros_control version of the driver, I switched to using the PositionJointInterface-based joint trajectory controller because I was suffering from pretty big stability issues when using the VelocityJointInteface-based joint trajectory controller (I reckon that this can be alleviated by tuning the PID parameters in the latter, but I figured that using the former was a quicker way to avoid the issue altogether).

When using the position interface in the driver, I see pretty noticeable overshoot, whether I use the joint_trajectory_controller in ros_controllers or other custom position controllers I've implemented myself. I don't think this comes from the controllers themselves, since the same controllers don't show this behavior when used to control a LWR. Since the ur_modern_driver does nothing more than forwarding the command from the controller to the servoing thread in the UR, I'm guessing this is some artifact of using servoj.

Has anyone else seen this effect and played a bit with the parameters to servoj to try to improve its performance?

Branch for kinetic?

hardware_interface has some ABI breaking changes between indigo and kinetic. I've updated ur_modern_driver to work with them in a branch in our fork: iron-ox@883070d

Those changes aren't backwards compatible with indigo though. If a new kinetic-devel branch is created, I can submit a PR.

Corrupted joint_states are published with ur_sim and real robot (1.8.x).

While using the driver to interface with both (but not at the same time) a ur-sim instance (v1.8.14035) and a real controller (CB2, also v1.8.14035), we sometimes observe JointState messages that have some really strange values in some of the position and velocity arrays. An example is pasted below:

header: 
  seq: 17396
  stamp: 
    secs: 1448973318
    nsecs: 95394434
  frame_id: ''
name: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
position: [0.0008817444019501054, 0.0, 0.0, 0.0, -4.737910152973887e+226, 3.4403246606906987e-246]
velocity: [5.356556342585614e-69, -4.555528633092074e-39, -3.501131509392264e-164, -4.0975983370358134e-95, 2.139376420684511e+253, 1.1701194070372343e-307]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 8.0963e-320]

Notice the -4.737910152973887e+226 and 3.4403246606906987e-246 in the position array.

Could this be an off-by-one error in the de-serialisation routines?

tool0_controller/tool0 frame offset

When I run roslaunch ur_modern_driver ur10_bringup.launch robot_ip:=... I get a weird position of the tool0_controller frame. It seems like that the tool0_controller frame is the mirrored version of tool0 (symmetrical around base frame z-axis).
Has anybody encountered this problem before?

Real UR10 with version 3.2.18654

Simulated UR10 with 3.2.19293

joint_state_interface.h included but not present

there seems to be includes for #include <hardware_interface/joint_state_interface.h>

which doesn't seem to exist in the include dir

respond to received data to handle Nagle

Make sure to send a newline quickack each time a package is received on both ports to force an ACK when we don't transmit data to the robot. Currently I set NODELAY, it should be TCP_QUICKACK ( https://github.com/ThomasTimm/ur_modern_driver/blob/master/src/ur_realtime_communication.cpp#L124 and https://github.com/ThomasTimm/ur_modern_driver/blob/master/src/ur_communication.cpp#L127 are the main culprints... Check for other in initializing code)

Occasional Jerky motion

UR5 running ployscope 3.2.18744. very minimal additional end effector mass, ubuntu 14.04 VM on windows host and ros-indigo. Both with test_move.py and when using the moveit_commander interface to plan and execute trajectories to ee goal positions, the arm occasionaly exhibits the odd jolt during execution, particularly for longer paths, when many joints are moving. Here is an example video:

Video

You see the jolt at roughly 3 seconds. This was performed with the moveit_commander interface.

Here is the output on follow_joint_trajectory/goal, that corresponds with the above video.

header: 
  seq: 9
  stamp: 
    secs: 1465575469
    nsecs: 202095307
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1465575469
    nsecs: 202096304
  id: /move_group-10-1465575469.202096304
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /world
    joint_names: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
    points: 
      - 
        positions: [-0.8290494124041956, -2.5253475348102015, -0.8362420240985315, 0.7118555307388306, -1.584247891102926, -1.302772347127096]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, -0.3327029212965289, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        positions: [-0.8957262914938168, -2.4424336433522873, -0.8870695181020145, 0.814757601131566, -1.5826221988115585, -1.3017115409826472]
        velocities: [-0.120625527129188, 0.15, -0.0919523130137044, 0.18616072997544747, 0.0029410493153477525, 0.0019191105238146062]
        accelerations: [0.0, 0.0, 3.7659579135098096e-16, 0.0, -7.27300622046582e-16, -7.269083347639247e-16]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 552759276
      - 
        positions: [-0.9624031705834379, -2.359519751894373, -0.9378970121054974, 0.9176596715243015, -1.5809965065201912, -1.3006507348381986]
        velocities: [-0.12062552712918757, 0.15, -0.09195231301370407, 0.18616072997544697, 0.0029410493153477443, 0.001919110523814601]
        accelerations: [1.5565959375840504e-15, 0.0, 8.787235131522865e-16, -1.8076597984847034e-15, 6.982713631299419e-16, 7.080785451963737e-16]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 105518552
      - 
        positions: [-1.029080049673059, -2.2766058604364585, -0.9887245061089802, 1.020561741917037, -1.5793708142288236, -1.2995899286937498]
        velocities: [-0.12062552712918767, 0.15, -0.09195231301370417, 0.18616072997544697, 0.0029410493153477443, 0.001919110523814802]
        accelerations: [-1.9080853428449648e-15, 0.0, -1.2553193045032663e-15, 1.8076597984847034e-15, -6.982713631299419e-16, 1.8829789567548997e-17]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 658277829
      - 
        positions: [-1.0957569287626803, -2.1936919689785443, -1.0395520001124632, 1.1234638123097724, -1.5777451219374563, -1.298529122549301]
        velocities: [-0.120625527129188, 0.15, -0.09195231301370452, 0.18616072997544747, 0.0029410493153477525, 0.0019191105238146062]
        accelerations: [7.280851966118966e-16, 0.0, 0.0, 0.0, 7.27300622046582e-16, -7.269083347639247e-16]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 211037105
      - 
        positions: [-1.1624338078523013, -2.11077807752063, -1.0903794941159461, 1.226365882702508, -1.5761194296460888, -1.2974683164048524]
        velocities: [-0.1206255271291878, 0.15, -0.09195231301370432, 0.18616072997544728, 0.0029410493153477525, 0.0019191105238146062]
        accelerations: [0.0, 0.0, 7.280851966118966e-16, -7.029788105218311e-16, -7.27300622046582e-16, 7.269083347639247e-16]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 763796381
      - 
        positions: [-1.2291106869419224, -2.027864186062716, -1.1412069881194289, 1.3292679530952431, -1.5744937373547214, -1.2964075102604036]
        velocities: [-0.12062552712918784, 0.14999999999999997, -0.09195231301370418, 0.18616072997544722, 0.002941049315347748, 0.0019191105238148044]
        accelerations: [-1.255319304503268e-16, -5.0212772180130727e-17, -2.510638609006536e-16, 5.021277218013072e-16, 7.123937053056047e-16, -9.807182066431782e-18]
        effort: []
        time_from_start: 
          secs: 3
          nsecs: 316555658
      - 
        positions: [-1.2957875660315437, -1.9449502946048016, -1.1920344821229119, 1.4321700234879788, -1.5728680450633539, -1.2953467041159548]
        velocities: [-0.12062552712918787, 0.14999999999999997, -0.09195231301370425, 0.18616072997544697, 0.002941049315347744, 0.001919110523814601]
        accelerations: [0.0, 0.0, 0.0, -1.456170393223789e-15, -7.2730062204658e-16, -7.269083347639226e-16]
        effort: []
        time_from_start: 
          secs: 3
          nsecs: 869314934
      - 
        positions: [-1.362464445121165, -1.8620364031468872, -1.2428619761263948, 1.535072093880714, -1.5712423527719865, -1.2942858979715062]
        velocities: [-0.12062552712918784, 0.14999999999999997, -0.09195231301370438, 0.18616072997544703, 0.002941049315347748, 0.0019191105238146036]
        accelerations: [1.255319304503268e-16, 5.0212772180130727e-17, -4.770213357112419e-16, 1.657021481944314e-15, 7.422075387875573e-16, 7.367155168303555e-16]
        effort: []
        time_from_start: 
          secs: 4
          nsecs: 422074211
      - 
        positions: [-1.429141324210786, -1.779122511688973, -1.2936894701298778, 1.6379741642734496, -1.569616660480619, -1.2932250918270574]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.4364486758786933, -0.542731732991224, 0.3327029212965289, -0.6735689039632393, -0.010641338611542983, -0.006943747869278039]
        effort: []
        time_from_start: 
          secs: 4
          nsecs: 974833487
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0

---

Communication problems UR3

Hello,

I'm having some communication problems while trying to send commands to the robot, sometimes even with the test_move.py file. I still don't know why sometimes it works and some others it can't connect to the server.

rosrun ur_modern_driver test_move.py
Waiting for server...

The same happens when I try to generate an actionlib on my code, it keeps trying to connect but does nothing more...

I am running the ur_modern_driver ur3_bringup.launch and it seems to work fine:

Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://leela:48102/
SUMMARY ========

PARAMETERS
* /robot_description: <?xml version="1....
* /rosdistro: indigo
* /rosversion: 1.11.19
* /ur_driver/base_frame: neck_base
* /ur_driver/max_payload: 3.0
* /ur_driver/max_velocity: 10.0
* /ur_driver/min_payload: 0.0
* /ur_driver/prefix: neck_
* /ur_driver/robot_ip_address: 192.168.102.62
* /ur_driver/servoj_time: 0.008
* /ur_driver/tool_frame: neck_tool0_contro...

NODES / robot_state_publisher (robot_state_publisher/robot_state_publisher) ur_driver (ur_modern_driver/ur_driver)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found process[robot_state_publisher-1]: started with pid [5204] process[ur_driver-2]: started with pid [5205] [ INFO] [1468499587.988261117]: Setting prefix to neck_

Any ideas of why is this happening? Am I missing something?

Thanks

Cannot cennect to Followjointtrajectoryaction in CPP for the ur_modern_driver

Hi,
Anyone has developed a action client in C++?
I transfer the test_move.py to a cpp file, but it does not work, it seems that it can not connect to the action server.
It stops at below.

while(!traj_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <std_msgs/String.h>
#include <control_msgs/FollowJointTrajectoryAction.h>

typedef actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction> TrajClient;

class RobotArm
{
private:
  // Action client for the joint trajectory action 
  // used to trigger the arm movement action
  TrajClient* traj_client_;

public:
  //! Initialize the action client and wait for action server to come up
  RobotArm() 
  {
    // tell the action client that we want to spin a thread by default
    traj_client_ = new TrajClient("follow_joint_trajectory", true);

    // wait for action server to come up
    while(!traj_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }
  }

  //! Clean up the action client
  ~RobotArm()
  {
    delete traj_client_;
  }

  //! Sends the command to start a given trajectory
  void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
  // void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
  {
    // When to start the trajectory: 1s from now
    goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
    traj_client_->sendGoal(goal);
  }

  //! Generates a simple trajectory with two waypoints, used as an example
  /*! Note that this trajectory contains two waypoints, joined together
      as a single trajectory. Alternatively, each of these waypoints could
      be in its own trajectory - a trajectory can have one or more waypoints
      depending on the desired application.
  */
  control_msgs::FollowJointTrajectoryGoal armExtensionTrajectory()
  {
    //our goal variable
    control_msgs::FollowJointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("elbow_joint");
    goal.trajectory.joint_names.push_back("wrist_1_joint");
    goal.trajectory.joint_names.push_back("wrist_2_joint");
    goal.trajectory.joint_names.push_back("wrist_3_joint");

    // We will have two waypoints in this goal trajectory
    goal.trajectory.points.resize(2);

    // First trajectory point
    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(6);

    goal.trajectory.points[ind].positions[0] = 2.2;
    goal.trajectory.points[ind].positions[1] = 0.0;
    goal.trajectory.points[ind].positions[2] = -1.57;
    goal.trajectory.points[ind].positions[3] = 0.0;
    goal.trajectory.points[ind].positions[4] = 0.0;
    goal.trajectory.points[ind].positions[5] = 1.0;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(6);
    for (size_t j = 0; j < 6; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 1 second after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);

    // Second trajectory point
    // Positions
    ind += 1;
    goal.trajectory.points[ind].positions.resize(6);
    goal.trajectory.points[ind].positions[0] = -0.3;
    goal.trajectory.points[ind].positions[1] = 0.2;
    goal.trajectory.points[ind].positions[2] = -0.1;
    goal.trajectory.points[ind].positions[3] = -1.2;
    goal.trajectory.points[ind].positions[4] = 1.5;
    goal.trajectory.points[ind].positions[5] = -0.3;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(6);
    for (size_t j = 0; j < 6; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 2 seconds after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(2.0);

    //we are done; return the goal
    return goal;
  }

  //! Returns the current state of the action
  actionlib::SimpleClientGoalState getState()
  {
    return traj_client_->getState();
  }

};

int main(int argc, char** argv)
{
  // Init the ROS node
  ros::init(argc, argv, "ur_driver");

  RobotArm arm;
  // Start the trajectory
  arm.startTrajectory(arm.armExtensionTrajectory());
  // Wait for trajectory completion
  while(!arm.getState().isDone() && ros::ok())
  {
    usleep(50000);
  }
}

I am just a beginner for ROS and UR.
And would you please tell how to intergrate other package such as ur_kinematics into test_move.py? where can I find an example for UR5?
Thanks for your help.

Sincerely,
Chunting

Stop the servoj thread on the controller when a trajectory is done

ros_control update timing

I used this project as a reference when I was implementing ros_control driver for my robotis arms. I found that I was unable to send a trajectory to the arm if I made use of the following code for the header of the trajectory

self._goal.trajectory.header.stamp = rospy.Time.now()

But the following worked, as this would set the header.stamp to zeros

self._goal.trajectory.header.stamp = rospy.Time(0)

I think this is because of the following lines in the ur_ros_wrapper.cpp where the controller->update is called, I think changing it to this would make it work. I haven't tried this out with a ur5, this is just my opinion based on using it in my code

controller_manager_->update(
                    ros::Time(current_time.tv_sec, current_time.tv_nsec),
                    elapsed_time);
// Changed to
controller_manager_->update(ros::Time::now(), elapsed_time);

Took me 3 days to debug this.. :(

problem with test_move.py and ur5_bringup.launch on CB3.2 Ur5

Hi,
When I run the rosrun ur_modern_driver test_move.py , it always stops at the "waiting for the server" and never continue. I program on UR5, CB 3.2.18744. UR5 IP is 192.168.1.102, and my PC is 192.168.1.100.

`2016-05-23 17:08:17 ☆  Chunting in ~/catkin_ws
○ → source devel/setup.bash 

 2016-05-23 17:08:29 ☆  Chunting in ~/catkin_ws
○ → rosrun ur_modern_driver test_move.py
Waiting for server...
^Z
[1]+  Stopped                 rosrun ur_modern_driver test_move.py`

What's more , when I run the command roslaunch ur_modern_driver ur5_bringup.launch robot_ip:=192.168.1.102 [reverse_port:=REVERSE_PORT]

**` 2016-05-23 17:15:59 ☆  Chunting in ~/catkin_ws
○ → roslaunch ur_modern_driver ur5_bringup.launch robot_ip:=192.168.1.102 [reverse_port:=REVERSE_PORT]
... logging to /home/chunting/.ros/log/bc99c3ea-20c5-11e6-bf75-44850011896a/roslaunch-Chunting-13638.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://Chunting:44301/

SUMMARY
========

PARAMETERS
 * /robot_description: <?xml version="1....
 * /rosdistro: indigo
 * /rosversion: 1.11.19
 * /ur_driver/base_frame: base_link
 * /ur_driver/max_payload: 5.0
 * /ur_driver/max_velocity: 10.0
 * /ur_driver/min_payload: 0.0
 * /ur_driver/prefix: 
 * /ur_driver/robot_ip_address: 192.168.1.102
 * /ur_driver/servoj_time: 0.008
 * /ur_driver/tool_frame: tool0_controller

NODES
  /
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    ur_driver (ur_modern_driver/ur_driver)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[robot_state_publisher-1]: started with pid [13659]
process[ur_driver-2]: started with pid [13660]
[FATAL] [1463995069.901632143]: Error connecting to get firmware version
terminate called after throwing an instance of 'std::system_error'
  what():  Invalid argument
[ur_driver-2] process has died [pid 13660, exit code -6, cmd /home/chunting/catkin_ws/devel/lib/ur_modern_driver/ur_driver __name:=ur_driver __log:=/home/chunting/.ros/log/bc99c3ea-20c5-11e6-bf75-44850011896a/ur_driver-2.log].
log file: /home/chunting/.ros/log/bc99c3ea-20c5-11e6-bf75-44850011896a/ur_driver-2*.log**

It does not succeed.

I can ping the robot.
Would you please tell me how correct it?

Thanks

Chunting
[email protected]

how do i teach trajectory to UR5 and log it

Low goal-accuracy of internal trajectory controller

I observed the internal trajectory controller (not using ros-control) is reaching a trajectory end-point only with low accuracy. I believe this might be related to servoj() which does not directly 'converge' at the goal-position (e.g. velocity is not 0 when robot moves to the servoj-position but passes through it which is of course the intended behavior during trajectory execution).

When using ros-control with position-control (which is during trajectory execution very similar to the internal trajectory controller, e.g. open-loop, only only forwarding interpolated positions) you see a clear 'over swinging' at the end of the trajectory, e.g. see our video at about 0:09 when the UR comes to a standstill. I guess the main difference between the internal trajectory-controller and ros-control is that ros-control keeps the control loop active (e.g. continues to send the goal servoj command on and on). In constrast to the current built-in joint_trajectory action implementation ros_control finally comes to the desired goal position with a minimal error.

Two possible solutions that come to my mind are:

• continue servoj until convergence to goal position
• trigger a movej with goal position after standstill

VelocityJointInterface unstable due to huge delay

As suggested by @ThomasTimm, I'm trying to use the VelocityJointInterface in the driver instead of PositionJointInterface. However, when starting the robot with the velocity-based joint trajectory controller, the robot quickly goes unstable, even without sending any command at all (the robot should try to keep the initial position via the PID in velocity_controllers::JointTrajectoryController, see here). Note that I'm not using any custom code here, just launching

$ roslaunch ur_modern_driver ur10_ros_control.launch

I modified the UR driver to broadcast more information in the joint state message (see this specific revision of ur_hardware_interface.cpp in my fork for details). The additional data I'm publishing is:

• target joint positions/velocities/currents as reported by the UR feedback messages
• target joint velocity as produced by the joint trajectory controller

What I observe is that there's a huge delay between the command written by the joint trajectory controller and the target velocity reported by the UR, as shown in the figure below, where the red line is the output of the controller, the light blue line is the target joint velocity as reported by the UR (i.e. what's returned by RobotStateRT::getQdTarget()), and the dark blue line is the actual joint velocity as reported by the UR driver (i.e. what's returned by RobotStateRT::getQdActual()).

I've tried with different controllers and lower PID gains (see here for more info), but the robot always goes unstable eventually. Additionally, the delay seems to increase as the driver runs. Lowering the PID gains makes the system go unstable more slowly, and after 40-50 seconds without any command (i.e. just holding the position) the delay between the controller command and the target speed reported by the UR grows beyond 2 seconds, as shown in the figure below.

I'm posting this here to see if anyone has had the same issue or can think of something I might be missing, since this is driving me crazy. @ThomasTimm do you actually work using the ros_control flavour of the driver or do you use it in the sans-ros_control mode? Have you seen anything like this before?

Interrupting trajectories makes it difficult to get goal start position right.

As noted in #35 and by some users via email, the driver's current strictness towards setting a correct starting pose of a trajectory can be difficult to get right if the robot is moving.

The idea of cheching this value is to ensure that the trajectory is still valid, so a message sent over a high-load network will not result in collisions because it is executed half a second to late.

Some possible workaround may include

• Use a bool rosparam strict-start to allow the user to turn the check on and off
  -what default value should we use?
• Use a double rosparam start-tolerance to allow the user to specify a tolerance of error
  • What should default value be? 0 for highest safety, 0.1 might be to sloppy?
• Insert the current pose as starting pose if the first point in the trajectory has a time_since_start value higher than 0
  • This would allow the easiest transition from the old driver and make it possible to have the safety check in place for the critical trajectories (by specifying a start pose) and have "sloppy" safety when not needed
• A combination of the above (what combination?)

@gavanderhoorn @shaun-edwards @ipa-fxm : any thoughts?

crash on using test move

Hello,

using ur5_bringup.launch looks ok, however when test_move.py is run, after hitting y to continue, the driver crashes with this result:

[roslaunch][INFO] 2016-03-09 19:36:45,064: process[ur_driver-2]: starting os process
[roslaunch][INFO] 2016-03-09 19:36:45,064: process[ur_driver-2]: start w/ args [[u'/home/administrator/catkin_ws$
[roslaunch][INFO] 2016-03-09 19:36:45,064: process[ur_driver-2]: cwd will be [/home/administrator/.ros]
[roslaunch][INFO] 2016-03-09 19:36:45,065: process[ur_driver-2]: started with pid [3936]
[roslaunch][INFO] 2016-03-09 19:36:45,065: ... successfully launched [ur_driver-2]
[roslaunch][INFO] 2016-03-09 19:36:45,065: ... launch_nodes complete
[roslaunch.pmon][INFO] 2016-03-09 19:36:45,066: registrations completed <ProcessMonitor(ProcessMonitor-1, starte$
[roslaunch.parent][INFO] 2016-03-09 19:36:45,066: ... roslaunch parent running, waiting for process exit
[roslaunch][INFO] 2016-03-09 19:36:45,066: spin
[roslaunch][ERROR] 2016-03-09 19:36:49,633: [ur_driver-2] process has died [pid 3936, exit code -11, cmd /home/a$
log file: /home/administrator/.ros/log/e9b182f4-e656-11e5-82ae-bd59d694d84e/ur_driver-2*.log
[roslaunch.pmon][INFO] 2016-03-09 19:36:49,633: ProcessMonitor.unregister[ur_driver-2] starting
[roslaunch.pmon][INFO] 2016-03-09 19:36:49,633: ProcessMonitor.unregister[ur_driver-2] complete

Transfer ur_modern_driver to ros-industrial/universal_robots?

As discussed with @ThomasTimm and @shaun-edwards and during ROS-I Developer Online Meeting, transfering this new driver to the ROS-I repo (https://github.com/ros-industrial/universal_robot) would be a great thing.

This issue is for discussion on how this can be done in the smoothest way.

@ThomasTimm
Is there any current issue or PR (I don't see any) that blocks the transfer, i.e. should be resolved prior to the transfer?

This tutorial shows how a repo can be moved between repos without loosing history.
Is there also a way to transfer issues to the new repo? I've seen thing like that, but I don't know how this can be done...

[Question] Cannot solve "Goal start doesnt match current pose"

Hi,

i am facing a new problem since i updatet my Robot and have to use this new driver.
My program works without any problems using the old version of the driver. I just changed the ur5_bringup to use the new one, like you wrote in the Wiki.

The problem in Detail:
I am sending new joint_values to the robot by using the FollowJointTrajectory controller with an goal to reach. With the old version everything was fine. I need a smooth and not stopping movement because i change the TCP position with an frequence of 60Hz with small steps. This worked very well.

When i am starting the robot and my programm, the driver reports "on goal" like with the old driver. But then, when i am starting the movement the robot does not move and the Error "Goal start doesnt match current pose" comes up. Sometime it moves, but only for some steps.
When I use MoveIt and RViz everything works fine, so i think it is something in my code but i cant see where the error could be.

Do i have to use the tool0_controller? I am using the Moveit-Services GetPositionFK and GetPositionIK for actual position and joint values with the chains from base_link to ee_link.

Because this doesnt come up when i used the old driver, i thought you can tell me what changes in my program i have to do or what action has the effect, that this Error comes up. I hope, starting an Issue was okay and that you can give me a hint to solve this problem.

Thank you very much

Julius

[question] will the old ur_driver/test_move.py work with the new driver without modification?

Just wondering if test_move.py has been ran with the new driver.

Change license

Change license to Apache for ROS-I compatibility

Mismatch in numbering digital pins between set_io and io_states

I noticed that the numbering of the digital output pins are different for the /io_states topic and the /set_io service call:

Service /set_io:
pin 0-7: Standard digital out
pin 8-9: Tool digital out
pin 10-17: Configurable digital out

rostopic /io_states:
pin 0-7: Standard digital out
pin 8-15: Configurable digital out
pin 16-17: Tool digital out

This mismatch is particularly annoying in case someone is checking the /io_states topic is feedback whether their service call has succesfully been carried out by UR.

Easiest fix seems to me to change the numbering in the /set_io service callback to use the same numbering as in the /io_states topic. I already have this fix ready.

I realize this may break existing applications because the pin numbers will be re-ordered. Should we still merge the fix?

Support for UR firmware 3.2

The introduction of firmware 3.2 adds a new field to the real time interface data stream.[1]
From the release notes:[2]

• Added digital outputs (address 131) to RealTimeClientInterface (if you use port 30003, it may affect your software)
• Added program state (address 132) to RealTimeClientInterface (if you use port 30003, it may affect your software)

This needs to be implemented.
At the same time it is worth considering a plan for handling the increasing fragmentation between versions, i.e. what should a getter return if an attempt is made to get a value which an older robot does not publish.

[1] http://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/remote-control-via-tcpip-16496/
[2] http://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/release-note-software-version-32xxxxx/

Attempting to write to socket that is closed

Solution:
Apply a check at https://github.com/ThomasTimm/ur_modern_driver/blob/master/src/ur_realtime_communication.cpp#L92 to see if the socket is connected before trying to write

Driver crashes when receiving a trajectory with only one point and time_from_start set to 0

Implement a check that verifies that the last point has a time_from_start larger than zero

ur5_ros_control totally unstable

Hello all,

Perhaps I am doing something wrong..
With a UR5 plugged in and working normally (Driver 3.2.18654), I execute:

roslaunch ur_modern_driver ur5_ros_control.launch robot_ip:=192.168.1.17

The robot then displays an unstable increasing oscillation in a random direction until I am forced to kill it.

Has the controller been tested on real robots?

Wrong length of message on RT interface

Trying to use this package with the following setup:

Client machine: Ubuntu 14.04.4
ROS version: Indigo
Robot: UR10
URControl/Polyscope version: 3.2.19293

The output of running the driver is below:

$ roslaunch ur_modern_driver ur10_bringup.launch robot_ip:=10.1.10.220
... logging to /home/yamokosk/.ros/log/66a35286-0639-11e6-b7a3-989096c96b47/roslaunch-beast-6486.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://beast:38766/

SUMMARY
========

PARAMETERS
 * /robot_description: <?xml version="1....
 * /rosdistro: indigo
 * /rosversion: 1.11.16
 * /ur_driver/base_frame: base_link
 * /ur_driver/max_payload: 10.0
 * /ur_driver/max_velocity: 10.0
 * /ur_driver/min_payload: 0.0
 * /ur_driver/prefix: 
 * /ur_driver/robot_ip_address: 10.1.10.220
 * /ur_driver/servoj_time: 0.008
 * /ur_driver/tool_frame: tool0_controller

NODES
  /
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    ur_driver (ur_modern_driver/ur_driver)

ROS_MASTER_URI=http://10.1.10.195:11311

core service [/rosout] found
process[robot_state_publisher-1]: started with pid [6344]
process[ur_driver-2]: started with pid [6345]
[DEBUG] [1461090622.377862880]: Max velocity accepted by ur_driver: 10.000000 [rad/s]
[DEBUG] [1461090622.383396234]: Min payload set to: 0.000000 [kg]
[DEBUG] [1461090622.389380645]: Max payload set to: 10.000000 [kg]
[DEBUG] [1461090622.389424038]: Bounds for set_payload service calls: [0.000000, 10.000000]
[DEBUG] [1461090622.394952805]: Servoj_time set to: 0.008000 [sec]
[DEBUG] [1461090622.399890538]: Base frame set to: base_link
[DEBUG] [1461090622.407237390]: Tool frame set to: tool0_controller
[DEBUG] [1461090622.407281113]: Acquire firmware version: Connecting...
[DEBUG] [1461090622.410286155]: Acquire firmware version: Got connection
[DEBUG] [1461090622.926672201]: Firmware version detected: 3.2019293
[DEBUG] [1461090622.926762164]: Switching to secondary interface for masterboard data: Connecting...
[DEBUG] [1461090622.928908912]: Secondary interface: Got connection
[DEBUG] [1461090622.929007812]: Realtime port: Connecting...
[DEBUG] [1461090622.931536234]: Listening on 10.1.10.149:50001

[DEBUG] [1461090622.931606611]: Realtime port: Got connection
[DEBUG] [1461090623.047969153]: The action server for this driver has been started
Wrong length of message on RT interface: -1077783229
Wrong length of message on RT interface: -1078231040
Wrong length of message on RT interface: 0
Wrong length of message on RT interface: -1077783229
Wrong length of message on RT interface: 0
^C[ur_driver-2] killing on exit
[robot_state_publisher-1] killing on exit
ur_driver: /usr/include/boost/thread/pthread/recursive_mutex.hpp:101: boost::recursive_mutex::~recursive_mutex(): Assertion `!pthread_mutex_destroy(&m)' iled.
shutting down processing monitor...
... shutting down processing monitor complete
done

Looking at the code, I don't think this is a "3.2 version" (#17) problem as it looks like the code accounts for that version. Or maybe I am wrong?

add lookahead_time to servoj command for firmware version > 3.1

https://github.com/ThomasTimm/ur_modern_driver/blob/master/src/ur_driver.cpp#L183

Not possible to set configurable IO from driver

Using the '/set_io' service, it is possible to set the digital output pints of the UR.
However, the UR also has 8 configurable output pins that can be used as digital output pins (but also for external emergency button, etc.)

It would be nice to extend the ur_driver/set_io interface to also set the configurable IO.
Note: the configurable pins are already visible through '/io_states', just setting them is not possible.

Setting is done in ur_driver.cpp at line 295:

void UrDriver::setDigitalOut(unsigned int n, bool b) {
    char buf[256];
    sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
            b ? "True" : "False");
    rt_interface_->addCommandToQueue(buf);
    print_debug(buf);
}

The 'set_digital_out' script function is used.
There is a UR script function specifically for setting the configurable IO's:

set_configurable_digital out(n, b)

I propose one of the following:

1. Use the existing service definition, but automatically set higher pins to configurable IO:

• pin 0-7: 'Normal' digital output pins
• pin 8-9: Tool output 0 and 1 (this already the case)
• pin 10-17: Configurable output pins (so pin 10 leads to: set_configurable_digital out(0, True))
  1. Extend the existing service definition to explicitly set configurable IO in ur_msgs/srv/SetIO.srv:

int8 FUN_SET_DIGITAL_OUT = 1
int8 FUN_SET_FLAG = 2
int8 FUN_SET_ANALOG_OUT = 3
int8 FUN_SET_TOOL_VOLTAGE = 4
int8 FUN_SET_CONFIGURABLE_OUT=5
int8 fun
int8 pin
float32 state

---
bool success

Both approaches have drawbacks I think. What would be the best way forward?

driver only executing a portion of the planned trajectory

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.

getQdTarget gets Qtarget

see line 135 of robot_state_RT.cpp. ret = q_target_ should be ret = qd_target_

Check for return value of socket_open in URScript

When using the driver with a simulated robot on a virtual box with poorly configured network, the driver might be able to see the robot and attempt a connect, but the box might not be able to connect to the driver with URScript.

A suggested solution is to check the return value of socket_open in the URScript and alert the user with a popup, if the reverse connection is unsuccessful.

This is seldom a problem (where the driver can connect to the robot and upload URScript, but the robot cannot open a reverse connection), and should be compared to the added amount of data to be transferred to the robot on each new trajectory. Thus this issue will probably remain open for some time to allow better solution suggestions.

Change minimum servoj time to 0.008 as firmware version > 3.0 appears to be able to handle this

Make it a constant in header file driver.h instead of a hardcoded "migic" number 3 places in driver.cpp