No effort feedback

[gtatiya]

Summary

Hi, I'm trying to get effort feedback from the UR5 robot, but I'm not getting it. I'm running this commands:

Terminal 1:
roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=172.22.22.2 kinematics_config:="$(rospack find bimur_ur_launch)/etc/bimur_right_arm_calibration.yaml"

Terminal 2:
rostopic echo /joint_states

Actual Behavior

Here's the output:

As you can see effort values are zeros. It's zero even if I move the arm.

I followed the instructions provided to set the driver up and it works as I'm able to control the arm using rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller.

I was getting effort values using the old driver which is deprecated: https://github.com/ros-industrial/ur_modern_driver.git.

Expected Behavior

Here's the output of rostopic echo /joint_states for old driver:

How can get the effort feedback?

Versions

• ROS Driver version: Kinetic
• Affected Robot Software Version(s): URSoftware 3.7
• Robot Serial Number: 2018359497
• UR+ product(s) installed: UR5
• URCaps Software version(s): External Control 1.0.1

[fmauch]

Yes, this was never implemented.

The reasoning for this is that the robot does not provide any effort information. The ur_modern_driver was publishing the joint currents to the effort field instead.

This being said, if you're only interested in getting the joint currents, this will be resolved in #35. If you would vote for having the joint currents in the effort field of the joint_state we can discuss that here :-)

[gavanderhoorn]

@fmauch wrote:

The reasoning for this is that the robot does not provide any effort information.

well, there is get_joint_torques() in URScript.

Not sure whether we can get the same information into the RTDE output?

[fmauch]

Well, there is target_moment but no actual field.

[gavanderhoorn]

Hm.

Could be worth a check with UR?

Aren't there "free registers" available for syncing arbitrary data with RTDE?

[fmauch]

Hm.

Could be worth a check with UR?

Definitely

Aren't there "free registers" available for syncing arbitrary data with RTDE?

Yes, we could certainly hack around this by wrapping the URScript into a general purpose register...

[gtatiya]

@fmauch @gavanderhoorn Thanks for answering.

My goal is to use effort feedback for object recognition as effort feedback would vary based in the object's weight. I've used real Baxter robot to do it and now I want to use UR5. I've not used the old UR5 driver, so I'm not sure that joint currents would be helpful or not. But, I'd prefer to use actual effort feedback because I'm working on a research project for my PhD.

Based on your responses, I've following questions as I'm not familiar with robot drivers a lot:

• @fmauch How to use solution in Rtde recipe publishing #35? I'm sorry, I'd need some more instructions to use this solution.

• @gavanderhoorn Where to find get_joint_torques() in URScript? I tried searching for it in new and old driver packages, but could not find it. Is it a
  URCaps Software?

• I'm totally unaware of target_moment, free registers, RTDE … But, maybe it's not important for me to know. Let me know if you think it is.

Do you guys think getting effort feedback is possible in UR5?

[fmauch]

• To make use of Rtde recipe publishing #35 you can try to checkout that branch and add the joint currents or target_moment fields to the output recipe file. This is currently a feature in development, so sorry that there isn't a ton of documentation there, yet.
• The RTDE protocol is used by this driver to receive information from the robot controller. See the documentation for available data fields. The difference between the actual_* and target_* fields is that the actual_* fields are measurements, where the target_* fields are setpoints sent to the underlying hardware controller.
• get_joint_torques() is a URScript function. Programs on the robot controller are written in URScript. This driver also adds a script code to a program running on the controller. This program could be modified, of course, but this requires some knowledge to do so. A start would be the URScript manual which you can download here.
  In theory this could be used to inject a piece of URScript that will call the get_joint_torques() method and saves the result into a general purpose field of RTDE. This field could then be read on the driver's side to be added to the joint_state.

Any of the methods above yields some torque-related information (as the currents are also torque-reated), while the third option is probably closest to what you want to do, but it also seems to be the most hacky one. As @gavanderhoorn it might make more sense to talk to UR about this directly, first, which I will do soon. If you would like to implement one of the above, any contribution on this is welcome!

[macsmitty]

I also want to access torque values. Unfortunately in #35 when adding the field target_moment i get the error:
No supported publisher for RTDE data field target_moment
and the process dies. Any pointer how to fix this would be highly appreciated.

Edit: There is a typo in data_field_publisher.cpp (target_movement instead of target_moment)

[fmauch]

After talking back with UR, there is no equivalent to get_joint_torques() in any of the interfaces.

The hack with injecting script code that would send this back to ROS would indeed work (e.g. by writing them to a RTDE general purpose register).

Another option is to take the actual_current and target_current RTDE fields and subtract them. This should be linear dependent on the actual joint torques.

@macsmitty Thanks for pointing out that typo. Could you please raise that on the actual PR itself? Unfortunately I haven't found the time to review this properly, yet.

[gavanderhoorn]

The hack with injecting script code that would send this back to ROS would indeed work (e.g. by writing them to a RTDE general purpose register).

This would seem like the most straightforward way to achieve what we want, right? We just pretend there is actually such a field in RTDE, and parse the result as if it were there.

If/when actual_joint_torque gets added, it would be a minimal change to no longer call the urscript function and just rely on the RTDE field directly.

[tonkei0361]

@fmauch As you mentioned that get_joint_torques() returns the torques on all joints, where is the output returned to? I don't know what a RTDE general purpose register is and how to access this register if it stores the output. Do I need to set up RTDE on my ROS pc in order to read the output of this script? What I was expecting was a topic or something equivalent that stores the value returned by a script but I could be wrong since I'm not very familiar with the driver:(

[fmauch]

A very brief architecture overview is given at the driver's README page.

When the driver and External Control are running, script code is being sent to the robot and executed there.

For achieving the above, you would have to modify this script code such that it runs another thread that calls get_joint_torques() in a loop and saves this data into a temporary variable.

Then (in the same loop) you can write each field of that variable into individual RTDE fields using write_output_float_register().

The code might look something like this (not tested, not complete):

thread torqueThread():
    while True:
        torques = get_joint_torques()
        write_output_float_register(24, torques[0])
        write_output_float_register(25, torques[1])
        write_output_float_register(26, torques[2])
        write_output_float_register(27, torques[3])
        write_output_float_register(28, torques[4])
        write_output_float_register(29, torques[5])
end
# Don't forget to run the thread and also care about killing / joining it. the `while True` probably is not the best method doing this.

This will populate the data to the RTDE fields output_double_register_24 to output_double_register_29.

You would have to add those to the output recipe and add handlers for that field to the HardwareInterface. As an example: This line reads a value to store it inside a member declared here and it gets published here (which is called here)

As you can see, this involves a lot of changes to get from a new RTDE field to a ROS topic, which is the main purpose of #35. If you like to implement this behaviour, I will be happy to review this PR.

[gtatiya]

@fmauch Thanks for your guidance.

I learned about URScript and also tried to understand your solution. But, I still need help to solve the problem:

• The 1st step is to modify the script code to add get_joint_torques(), but I'm not sure how it is getting the other data such as actual_q; get_actual_joint_positions() is not in a loop: link, and there is nothing called actual_q in the script, so how output recipe gets actual_q?

• Nothing in the script code is using output_double_register, so I'm not sure where to use write_output_float_register(24, torques[0])?

• I'd like to contribute, but Rtde recipe publishing #35 is a fork of this repo. and does not modify the ros_control.urscrip. I'm not using this repo., how can I directly contribute to the original repo.?

[fmauch]

The other fields used are already inside the RTDE protocol which is why you can't find them in the script code.

You can define the thread as mentioned above (similar to how the speed thread is defined) and then start the thread e.g. here by adding the line

torque_thread = run torqueThread()

Then, you'll have to also make sure to kill the thread at the end, e.g. after this line by adding the line

kill torque_thread

[gtatiya]

@fmauch I modified the script code as you suggested and ran roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=127.0.0.1 to test if it works as before, but I got this error:

The original script still works fine. Here's the modified script, it only has 150 lines. What could be wrong?

{{BEGIN_REPLACE}}

steptime = get_steptime()

textmsg("steptime=", steptime)
MULT_jointstate = {{JOINT_STATE_REPLACE}}

#Constants
SERVO_UNINITIALIZED = -1
SERVO_IDLE = 0
SERVO_RUNNING = 1

MODE_STOPPED = -2
MODE_UNINITIALIZED = -1
MODE_IDLE = 0
MODE_SERVOJ = 1
MODE_SPEEDJ = 2

#Global variables are also showed in the Teach pendants variable list
global cmd_servo_state = SERVO_UNINITIALIZED
global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
global cmd_servo_q = get_actual_joint_positions()
global cmd_servo_q_last = get_actual_joint_positions()
global extrapolate_count = 0
global extrapolate_max_count = 0
global control_mode = MODE_UNINITIALIZED
cmd_speedj_active = True

def set_servo_setpoint(q):
  cmd_servo_state = SERVO_RUNNING
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = q
end

def extrapolate():
  diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]

  return cmd_servo_q
end

thread servoThread():
  textmsg("Starting servo thread")
  state = SERVO_IDLE
  while control_mode == MODE_SERVOJ:
    enter_critical
    q = cmd_servo_q
    do_extrapolate = False
    if (cmd_servo_state == SERVO_IDLE):
      do_extrapolate = True
    end
    state = cmd_servo_state
    if cmd_servo_state > SERVO_UNINITIALIZED:
      cmd_servo_state = SERVO_IDLE
    end

    if do_extrapolate:
      extrapolate_count = extrapolate_count + 1
      if extrapolate_count > extrapolate_max_count:
        extrapolate_max_count = extrapolate_count
      end

      q = extrapolate()
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})

    elif state == SERVO_RUNNING:
      extrapolate_count = 0
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
    else:
      extrapolate_count = 0
      sync()
    end
    exit_critical
  end
  textmsg("servo thread ended")
  stopj(4.0)
end

# Helpers for speed control
def set_speed(qd):
  cmd_servo_qd = qd
  control_mode = MODE_SPEEDJ
end

thread speedThread():
  textmsg("Starting speed thread")
  while control_mode == MODE_SPEEDJ:
    qd = cmd_servo_qd
    speedj(qd, 40.0, steptime)
  end
  textmsg("speedj thread ended")
  stopj(5.0)
end

thread torqueThread():
    while True:
        torques = get_joint_torques()
        write_output_float_register(24, torques[0])
        write_output_float_register(25, torques[1])
        write_output_float_register(26, torques[2])
        write_output_float_register(27, torques[3])
        write_output_float_register(28, torques[4])
        write_output_float_register(29, torques[5])
end

socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")

torque_thread = run torqueThread()

control_mode = MODE_UNINITIALIZED
thread_move = 0
global keepalive = -2
textmsg("External control active")
params_mult = socket_read_binary_integer(1+6+1, "reverse_socket")
keepalive = params_mult[1]
while keepalive > 0 and control_mode > MODE_STOPPED:
  enter_critical
  socket_send_line(1, "reverse_socket")
  params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
  if params_mult[0] > 0:
    keepalive = params_mult[1]
    if control_mode != params_mult[8]:
      control_mode = params_mult[8]
      join thread_move
      if control_mode == MODE_SERVOJ:
        thread_move = run servoThread()
      elif control_mode == MODE_SPEEDJ:
        thread_move = run speedThread()
      end
    end
    if control_mode == MODE_SERVOJ:
      q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
      set_servo_setpoint(q)
    elif control_mode == MODE_SPEEDJ:
      qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
      set_speed(qd)
    end
  else:
    keepalive = keepalive - 1
  end
  exit_critical
end

textmsg("Stopping communication and control")
control_mode = MODE_STOPPED
join thread_move
kill torque_thread
textmsg("All threads ended")
socket_close("reverse_socket")

[fmauch]

As I said, I didn't test the code, just wrote it by heart. This code seems to work:

{{BEGIN_REPLACE}}

steptime = get_steptime()

textmsg("steptime=", steptime)
MULT_jointstate = {{JOINT_STATE_REPLACE}}

#Constants
SERVO_UNINITIALIZED = -1
SERVO_IDLE = 0
SERVO_RUNNING = 1

MODE_STOPPED = -2
MODE_UNINITIALIZED = -1
MODE_IDLE = 0
MODE_SERVOJ = 1
MODE_SPEEDJ = 2

#Global variables are also showed in the Teach pendants variable list
global cmd_servo_state = SERVO_UNINITIALIZED
global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
global cmd_servo_q = get_actual_joint_positions()
global cmd_servo_q_last = get_actual_joint_positions()
global extrapolate_count = 0
global extrapolate_max_count = 0
global control_mode = MODE_UNINITIALIZED
cmd_speedj_active = True

def set_servo_setpoint(q):
  cmd_servo_state = SERVO_RUNNING
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = q
end

def extrapolate():
  diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]

  return cmd_servo_q
end

thread servoThread():
  textmsg("Starting servo thread")
  state = SERVO_IDLE
  while control_mode == MODE_SERVOJ:
    enter_critical
    q = cmd_servo_q
    do_extrapolate = False
    if (cmd_servo_state == SERVO_IDLE):
      do_extrapolate = True
    end
    state = cmd_servo_state
    if cmd_servo_state > SERVO_UNINITIALIZED:
      cmd_servo_state = SERVO_IDLE
    end

    if do_extrapolate:
      extrapolate_count = extrapolate_count + 1
      if extrapolate_count > extrapolate_max_count:
        extrapolate_max_count = extrapolate_count
      end

      q = extrapolate()
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})

    elif state == SERVO_RUNNING:
      extrapolate_count = 0
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
    else:
      extrapolate_count = 0
      sync()
    end
    exit_critical
  end
  textmsg("servo thread ended")
  stopj(4.0)
end

# Helpers for speed control
def set_speed(qd):
  cmd_servo_qd = qd
  control_mode = MODE_SPEEDJ
end

thread speedThread():
  textmsg("Starting speed thread")
  while control_mode == MODE_SPEEDJ:
    qd = cmd_servo_qd
    speedj(qd, 40.0, steptime)
  end
  textmsg("speedj thread ended")
  stopj(5.0)
end

thread torqueThread():
  while True:
    torques = get_joint_torques()
    write_output_float_register(4, torques[0])
    write_output_float_register(5, torques[1])
    write_output_float_register(6, torques[2])
    write_output_float_register(7, torques[3])
    write_output_float_register(8, torques[4])
    write_output_float_register(9, torques[5])
    sync()
  end
end

socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")

torque_thread = run torqueThread()

control_mode = MODE_UNINITIALIZED
thread_move = 0
global keepalive = -2
textmsg("External control active")
params_mult = socket_read_binary_integer(1+6+1, "reverse_socket")
keepalive = params_mult[1]
while keepalive > 0 and control_mode > MODE_STOPPED:
  enter_critical
  socket_send_line(1, "reverse_socket")
  params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
  if params_mult[0] > 0:
    keepalive = params_mult[1]
    if control_mode != params_mult[8]:
      control_mode = params_mult[8]
      join thread_move
      if control_mode == MODE_SERVOJ:
        thread_move = run servoThread()
      elif control_mode == MODE_SPEEDJ:
        thread_move = run speedThread()
      end
    end
    if control_mode == MODE_SERVOJ:
      q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
      set_servo_setpoint(q)
    elif control_mode == MODE_SPEEDJ:
      qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
      set_speed(qd)
    end
  else:
    keepalive = keepalive - 1
  end
  exit_critical
end

textmsg("Stopping communication and control")
control_mode = MODE_STOPPED
join thread_move
kill torque_thread
textmsg("All threads ended")
socket_close("reverse_socket")

[fmauch]

As I didn't have the time to answer properly yesterday:

The line number that came up in your error popup refers to the URScript generated by the program created on the robot. You can watch it by looking into the program folder on the robot. For your program my_program there will be multiple files, for example my_program.urscript which will contain the generated script code of your program.

In this case the error was referring to the last line, as I forgot to close the while loop with an end tag leading to an unclosed section at the end of the script.

Furthermore, the robot was complaining about index 24 of the float registers couldn't be written to. Maybe I understood the documentation wrong, there.

Anyway, that should cover the URScript part of your effort feedback.

[gavanderhoorn]

It would be great if the efforts of @gtatiya would result in a PR which contributes proper effort output on the joint_states topic.

[gtatiya]

@fmauch, @gavanderhoorn Could we work on it here: #160

[Scott-Merritt]

@fmauch Does this issue address commanding efforts for full feedback control of the UR arms, or does it only apply to reading them?

I've been digging pretty deep into URScript and RTDE documentation and can't seem to find a way to command torque at all. It seems like commanding torque (i.e., implementing a JointEffortInterface) is not possible? Or am I missing something?

[gavanderhoorn]

@Scott-Merritt: no, this PR will not let you command joint torques.

I've been digging pretty deep into URScript and RTDE documentation and can't seem to find a way to command torque at all. It seems like commanding torque (i.e., implementing a JointEffortInterface) is not possible? Or am I missing something?

No. Afaik, there is no publicly available interface which allows you to command joint torques for URs.

[gtatiya]

This issue is solved in commit b46651. Thanks!

[Scott-Merritt]

@gavanderhoorn @fmauch

No. Afaik, there is no publicly available interface which allows you to command joint torques for URs.

Are you aware of a private interface that does so? Are you aware of UR providing access to such an interface to their partners? I'm trying to gauge whether it would be a worthwhile use of time to work my way up through the sales channels, or if that's ultimately going to be a dead end.