old_api.py
Go to the documentation of this file.
1 # -*- coding: utf-8 -*-
2 
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2017, Felix von Drigalski
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of JSK Lab, University of Tokyo. nor the
19 # names of its contributors may be used to endorse or promote products
20 # derived from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34 
35 # This module extends the HIRONX client class with some functions
36 # from the original HIRO API, so that old code can be ported more easily.
37 # Usage in an old script:
38 # 1) Add these at the start of your file
39 # from hironx_ros_bridge import old_api
40 # robot = hironx_client.HIRONX()
41 #
42 # 2) Go through the code and replace functions like this:
43 # moveRelativeL(dz=0.05) ==> robot.moveRelativeL(dz=0.05)
44 # getCurrentConfiguration(armL_svc) ==> robot.getCurrentConfiguration('LARM_JOINT5')
45 
46 
47 def setTargetAngular(self, group_name, x, y, z, r, p, w, rate=10.0, wait=True):
48  '''
49  This is only an approximation of the original function. It can be
50  considerably slower for small movements that contain rotation.
51  '''
52  if group_name == 'larm':
53  joint_name = 'LARM_JOINT5'
54  elif group_name == 'rarm':
55  joint_name = 'RARM_JOINT5'
56 
57  scaling_factor_translation = 50
58  scaling_factor_rotation = 5
59  x_now, y_now, z_now, r_now, p_now, w_now = self.getCurrentConfiguration(joint_name)
60  translation_distances = map(abs, [x-x_now * scaling_factor_translation,
61  y-y_now * scaling_factor_translation,
62  z-z_now * scaling_factor_translation])
63  rotation_distances = map(abs, [r-r_now * scaling_factor_rotation,
64  p-p_now * scaling_factor_rotation,
65  w-w_now * scaling_factor_rotation])
66  max_dist = max(translation_distances + rotation_distances) # This is just concatenated
67  mvmt_time = max_dist / rate
68 
69  pos = [x, y, z]
70  rpw = [r, p, w]
71  ret = self.setTargetPose(group_name, pos, rpw, mvmt_time)
72  if ret and wait:
73  self.waitInterpolationOfGroup(group_name)
74 
75  return ret
76 
77 def move(self, group_name, x, y, z, r, p, w, rate, wait = True):
78  return self.setTargetAngular(group_name, x, y, z, r, p, w, rate, wait)
79 
80 def moveR(self, x, y, z, r, p, w, rate = 10, wait = True):
81  return self.move('rarm', x, y, z, r, p, w, rate, wait)
82 
83 def moveL(self, x, y, z, r, p, w, rate = 10, wait = True):
84  return self.move('larm', x, y, z, r, p, w, rate, wait)
85 
86 def moveRelative(self, group_name, joint_name, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait=True):
87  pos = self.getCurrentPosition(joint_name)
88  rpw = self.getCurrentRPY(joint_name)
89  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)
90 
91 def moveRelativeR(self, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait = True):
92  return self.moveRelative('rarm', 'RARM_JOINT5', dx, dy, dz, dr, dp, dw, rate, wait)
93 
94 def moveRelativeL(self,dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate = 8, wait = True):
95  return self.moveRelative('larm', 'LARM_JOINT5', dx, dy, dz, dr, dp, dw, rate, wait)
96 
97 def getCurrentConfiguration(self, joint_name):
98  xyz = self.getCurrentPosition(joint_name)
99  rpw = self.getCurrentRPY(joint_name)
100  return xyz[0], xyz[1], xyz[2], rpw[0], rpw[1], rpw[2]
101 
102 from hironx_ros_bridge import hironx_client
103 
104 # https://stackoverflow.com/questions/9455111/python-define-method-outside-of-class-definition
105 hironx_client.HIRONX.setTargetAngular = setTargetAngular
106 hironx_client.HIRONX.move = move
107 hironx_client.HIRONX.moveR = moveR
108 hironx_client.HIRONX.moveL = moveL
109 hironx_client.HIRONX.moveRelative = moveRelative
110 hironx_client.HIRONX.moveRelativeR = moveRelativeR
111 hironx_client.HIRONX.moveRelativeL = moveRelativeL
112 hironx_client.HIRONX.getCurrentConfiguration = getCurrentConfiguration


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Mon Feb 28 2022 23:45:15