Merge pull request #521 from k-okada/support_old_api

add python file to support old api

hironx_ros_bridge/src/hironx_ros_bridge/old_api.py

@@ -0,0 +1,112 @@
+# -*- coding: utf-8 -*-
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2017, Felix von Drigalski
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of JSK Lab, University of Tokyo. nor the
+#    names of its contributors may be used to endorse or promote products
+#    derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# This module extends the HIRONX client class with some functions
+# from the original HIRO API, so that old code can be ported more easily.
+# Usage in an old script:
+# 1) Add these at the start of your file
+# from hironx_ros_bridge import old_api
+# robot = hironx_client.HIRONX()
+#
+# 2) Go through the code and replace functions like this:
+# moveRelativeL(dz=0.05)              ==>      robot.moveRelativeL(dz=0.05)
+# getCurrentConfiguration(armL_svc)   ==>      robot.getCurrentConfiguration('LARM_JOINT5')
+
+
+def setTargetAngular(self, group_name, x, y, z, r, p, w, rate=10.0, wait=True):
+    ''' 
+    This is only an approximation of the original function. It can be
+    considerably slower for small movements that contain rotation.
+    '''
+    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]
+
+from hironx_ros_bridge import hironx_client
+
+# https://stackoverflow.com/questions/9455111/python-define-method-outside-of-class-definition
+hironx_client.HIRONX.setTargetAngular = setTargetAngular
+hironx_client.HIRONX.move = move
+hironx_client.HIRONX.moveR = moveR
+hironx_client.HIRONX.moveL = moveL
+hironx_client.HIRONX.moveRelative = moveRelative
+hironx_client.HIRONX.moveRelativeR = moveRelativeR
+hironx_client.HIRONX.moveRelativeL = moveRelativeL
+hironx_client.HIRONX.getCurrentConfiguration = getCurrentConfiguration