old_api.py
Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2017, Felix von Drigalski
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of JSK Lab, University of Tokyo. nor the
00019 #    names of its contributors may be used to endorse or promote products
00020 #    derived from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 # This module extends the HIRONX client class with some functions
00036 # from the original HIRO API, so that old code can be ported more easily.
00037 # Usage in an old script:
00038 # 1) Add these at the start of your file
00039 # from hironx_ros_bridge import old_api
00040 # robot = hironx_client.HIRONX()
00041 #
00042 # 2) Go through the code and replace functions like this:
00043 # moveRelativeL(dz=0.05)              ==>      robot.moveRelativeL(dz=0.05)
00044 # getCurrentConfiguration(armL_svc)   ==>      robot.getCurrentConfiguration('LARM_JOINT5')
00045 
00046 
00047 def setTargetAngular(self, group_name, x, y, z, r, p, w, rate=10.0, wait=True):
00048     ''' 
00049     This is only an approximation of the original function. It can be
00050     considerably slower for small movements that contain rotation.
00051     '''
00052     if group_name == 'larm':
00053         joint_name = 'LARM_JOINT5'
00054     elif group_name == 'rarm':
00055         joint_name = 'RARM_JOINT5'
00056 
00057     scaling_factor_translation = 50
00058     scaling_factor_rotation = 5
00059     x_now, y_now, z_now, r_now, p_now, w_now = self.getCurrentConfiguration(joint_name)
00060     translation_distances = map(abs, [x-x_now * scaling_factor_translation,
00061                                       y-y_now * scaling_factor_translation,
00062                                       z-z_now * scaling_factor_translation])
00063     rotation_distances = map(abs, [r-r_now * scaling_factor_rotation,
00064                                    p-p_now * scaling_factor_rotation,
00065                                    w-w_now * scaling_factor_rotation])
00066     max_dist = max(translation_distances + rotation_distances)  # This is just concatenated
00067     mvmt_time = max_dist / rate
00068 
00069     pos = [x, y, z]
00070     rpw = [r, p, w]
00071     ret = self.setTargetPose(group_name, pos, rpw, mvmt_time)
00072     if ret and wait:
00073         self.waitInterpolationOfGroup(group_name)
00074 
00075     return ret
00076 
00077 def move(self, group_name, x, y, z, r, p, w, rate, wait = True):
00078     return self.setTargetAngular(group_name, x, y, z, r, p, w, rate, wait)
00079 
00080 def moveR(self, x, y, z, r, p, w, rate = 10, wait = True):
00081     return self.move('rarm', x, y, z, r, p, w, rate, wait)
00082 
00083 def moveL(self, x, y, z, r, p, w, rate = 10, wait = True):
00084     return self.move('larm', x, y, z, r, p, w, rate, wait)
00085 
00086 def moveRelative(self, group_name, joint_name, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait=True):
00087     pos = self.getCurrentPosition(joint_name)
00088     rpw = self.getCurrentRPY(joint_name)
00089     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)
00090 
00091 def moveRelativeR(self, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait = True):
00092     return self.moveRelative('rarm', 'RARM_JOINT5', dx, dy, dz, dr, dp, dw, rate, wait)
00093 
00094 def moveRelativeL(self,dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait = True):
00095     return self.moveRelative('larm', 'LARM_JOINT5', dx, dy, dz, dr, dp, dw, rate, wait)
00096 
00097 def getCurrentConfiguration(self, joint_name):
00098     xyz = self.getCurrentPosition(joint_name)
00099     rpw = self.getCurrentRPY(joint_name)
00100     return xyz[0], xyz[1], xyz[2], rpw[0], rpw[1], rpw[2]
00101 
00102 from hironx_ros_bridge import hironx_client
00103 
00104 # https://stackoverflow.com/questions/9455111/python-define-method-outside-of-class-definition
00105 hironx_client.HIRONX.setTargetAngular = setTargetAngular
00106 hironx_client.HIRONX.move = move
00107 hironx_client.HIRONX.moveR = moveR
00108 hironx_client.HIRONX.moveL = moveL
00109 hironx_client.HIRONX.moveRelative = moveRelative
00110 hironx_client.HIRONX.moveRelativeR = moveRelativeR
00111 hironx_client.HIRONX.moveRelativeL = moveRelativeL
00112 hironx_client.HIRONX.getCurrentConfiguration = getCurrentConfiguration


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37