setTargetAngular, ArmControl service, rate parameter, porting old code

[felixvd]

We have recently upgraded our HIRONX to ROS and until then have used the setTargetAngular function of the ArmControlService to move the arms. It uses a "rate" parameter to set the movement speed, while the functions available in rtmros_hironx only offer a "time" parameter to set the total time the movement should take.

I read around in the code a bit and found that while it seemed to be used in the past, the hironx_client class inherits from HrpSysConfigurator, where the ArmControlService is not initialized anymore. It also seems like you reported similar issues before, which is why I am posting this here.

Do you have any advice on how to deal with porting old code to rtmros_hironx, specifically with the "rate" parameter? Is there anything else good to know about the codebase that you could share regarding this?

Thanks in advance!

[k-okada]

Hi, Sorry that I do not know/remember the exact meaning of 'rate' parameter, but basically you can control the setTargetPose with time parameter, may be you specify the default total time, say tm = 10; [sec], then pass to the function such that 'tm * rate', if the rate is defined between 0 - 1.

-- ◉ Kei Okada

2017-11-03 20:49 GMT+09:00 Felix von Drigalski notifications@github.com:

We have recently upgraded our HIRONX to ROS and until then have used the setTargetAngular function of the ArmControlService to move the arms. It uses a "rate" parameter to set the movement speed, while the functions available in rtmros_hironx only offer a "time" parameter to set the total time the movement should take.

I read around in the code a bit and found that while it seemed to be used in the past http://docs.ros.org/groovy/api/hironx_ros_bridge_old/html/gui_2sample_8py_source.html, the hironx_client class https://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py inherits from HrpSysConfigurator https://github.com/fkanehiro/hrpsys-base/blob/master/python/hrpsys_config.py, where the ArmControlService is not initialized anymore. It also seems like you reported similar issues https://github.com/fkanehiro/hrpsys-base/issues/104 before, which is why I am posting this here.

Do you have any advice on how to deal with porting old code to rtmros_hironx, specifically with the "rate" parameter? Is there anything else good to know about the codebase that you could share regarding this?

Thanks in advance!

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/start-jsk/rtmros_hironx/issues/520, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3IgYl9n1QWUSHfpgR76PnOpxeqTkks5syv3UgaJpZM4QQ-mQ .

[felixvd]

Thanks for your quick response. I thought of that, but sadly that will not work for our code. There are both very small and fairly long movements in it, so the resulting speed can be either much too slow or dangerously fast.

We can check the distance between the current and target joint configuration and set the time/rate accordingly. It shouldn't be too hard. I will post the function here if we do.

[felixvd]

To explain a bit: like I mentioned, we needed something with which we could reuse our old code for the HIRONX, so we extended the hironx_client class with the functions from the old interface (starting from line 202). With this, we can open an old HIRONX script, instantiate the class at the start, replace all the calls to the class method (e.g. "moveL(...)" -> "robot.moveL(...)"), and thus port the old code to work with this repository. In retrospect, I guess we could have defined the functions to use the hironx_client and avoid changing the old script at all.

Anyway, I pasted our code below in case someone has a similar problem. The only really new part is the setTargetAngular function, named after the one that used to be provided by the ArmControl service. In our preliminary tests, it behaves similar to the original. It may go too slow when a rotation sign changes during a small movement, but it should be safe.

I can make a pull request if there's interest.

  def setTargetAngular(self, group_name, x, y, z, r, p, w, rate=10.0, wait=True):
    if group_name == 'larm':
      joint_name = 'LARM_JOINT5'
    elif group_name == 'rarm':
      joint_name = 'RARM_JOINT5'

    scaling_factor_translation = 50
    scaling_factor_rotation = 5
    x_now, y_now, z_now, r_now, p_now, w_now = self.getCurrentConfiguration(joint_name)
    translation_distances = map(abs, [x-x_now * scaling_factor_translation,
              y-y_now * scaling_factor_translation,
              z-z_now * scaling_factor_translation])
    rotation_distances = map(abs, [r-r_now * scaling_factor_rotation,
              p-p_now * scaling_factor_rotation,
              w-w_now * scaling_factor_rotation])
    max_dist = max(translation_distances + rotation_distances)  # This is just concatenated
    mvmt_time = max_dist / rate

    pos = [x, y, z]
    rpw = [r, p, w]
    ret = self.setTargetPose(group_name, pos, rpw, mvmt_time)
    if ret and wait:
      self.waitInterpolationOfGroup(group_name)

    return ret

  def move(self, group_name, x, y, z, r, p, w, rate, wait = True):
    return self.setTargetAngular(group_name, x, y, z, r, p, w, rate, wait)

  def moveR(self, x, y, z, r, p, w, rate = 10, wait = True):
    return self.move('rarm', x, y, z, r, p, w, rate, wait)

  def moveL(self, x, y, z, r, p, w, rate = 10, wait = True):
    return self.move('larm', x, y, z, r, p, w, rate, wait)

  def moveRelative(self, group_name, joint_name, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait=True):
    pos = self.getCurrentPosition(joint_name)
    rpw = self.getCurrentRPY(joint_name)
    return self.move(group_name, pos[0] + dx, pos[1] + dy , pos[2] + dz , rpw[0] + dr, rpw[1] + dp, rpw[2] + dw, rate, wait) 

  def moveRelativeR(self, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait = True):
    return self.moveRelative('rarm', 'RARM_JOINT5', dx, dy, dz, dr, dp, dw, rate, wait)

  def moveRelativeL(self,dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait = True):
    return self.moveRelative('larm', 'LARM_JOINT5', dx, dy, dz, dr, dp, dw, rate, wait)

  def getCurrentConfiguration(self, joint_name):
    xyz = self.getCurrentPosition(joint_name)
    rpw = self.getCurrentRPY(joint_name)

    return xyz[0], xyz[1], xyz[2], rpw[0], rpw[1], rpw[2]

[k-okada]

@felixvd Thanks for suggestion, I have created PR #521, and expected use-case is something like

>>>  from hironx_ros_bridge import old_api
>>>  robot = hironx_client.HIRONX()

how do you think? if you have any suggestion ? please add patch against #521 branch

[felixvd]

That's a very good idea and much simpler than extending the class. I was playing around with another setup where the functions are not defined in the class, but like this:

from hironx_ros_bridge import hironx_client

robot = hironx_client.HIRONX()
[...]
def moveRelativeL(self,dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait = True):
    return robot.moveRelative('larm', 'LARM_JOINT5', dx, dy, dz, dr, dp, dw, rate, wait)
[...]

This way you could import it with from hironx_ros_bridge.old_api import * and use it without changing move commands in the script. However, it is not as clean, and old scripts still need to be cleaned anyway, so it does not help. I added comments to your pull request with short instructions, but it would make sense to note this in the documentation somewhere.

[felixvd]

I think this got solved with the PR.