hironx_client.py
Go to the documentation of this file.
1 # -*- coding: utf-8 -*-
2 
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2013, JSK Lab, University of Tokyo
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 import copy
36 import math
37 import numpy
38 import os
39 import time
40 
41 import roslib
42 roslib.load_manifest("hrpsys")
43 from hrpsys.hrpsys_config import *
44 
45 # hot fix for https://github.com/start-jsk/rtmros_hironx/issues/539
46 # On 16.04 (omniorb4-dev 4.1) if we start openhrp-model-loader with
47 # <env name="ORBgiopMaxMsgSize" value="2147483648" />
48 # all connection(?) conect respenct giopMaxMsgSize
49 # But on 18.04 (omniorb4-dev 4.2) wnneed to set ORBgiopMaxMsgSize=2147483648
50 # for each clients
51 if not os.environ.has_key('ORBgiopMaxMsgSize'):
52  os.environ['ORBgiopMaxMsgSize'] = '2147483648'
53 
54 import OpenHRP
55 import OpenRTM_aist
57 import rtm
58 from waitInput import waitInputConfirm, waitInputSelect
59 
60 from distutils.version import StrictVersion
61 
62 SWITCH_ON = OpenHRP.RobotHardwareService.SWITCH_ON
63 SWITCH_OFF = OpenHRP.RobotHardwareService.SWITCH_OFF
64 _MSG_ASK_ISSUEREPORT = 'Your report to ' + \
65  'https://github.com/start-jsk/rtmros_hironx/issues ' + \
66  'about the issue you are seeing is appreciated.'
67 _MSG_RESTART_QNX = 'You may want to restart QNX/ControllerBox afterward'
68 
69 def delete_module(modname, paranoid=None):
70  from sys import modules
71  try:
72  thismod = modules[modname]
73  except KeyError:
74  raise ValueError(modname)
75  these_symbols = dir(thismod)
76  if paranoid:
77  try:
78  paranoid[:] # sequence support
79  except:
80  raise ValueError('must supply a finite list for paranoid')
81  else:
82  these_symbols = paranoid[:]
83  del modules[modname]
84  for mod in modules.values():
85  try:
86  delattr(mod, modname)
87  except AttributeError:
88  pass
89  if paranoid:
90  for symbol in these_symbols:
91  if symbol[:2] == '__': # ignore special symbols
92  continue
93  try:
94  delattr(mod, symbol)
95  except AttributeError:
96  pass
97 
98 class HrpsysConfigurator2(HrpsysConfigurator):
99  default_frame_name = 'WAIST'
100 
101  def _get_geometry(self, method, frame_name=None):
102  '''!@brief
103  A method only inteded for class-internal usage.
104 
105  @since: 315.12.1 or higher
106  @type method: A Python function object.
107  @param method: One of the following Python function objects defined in class HrpsysConfigurator:
108  [getCurrentPose, getCurrentPosition, getCurrentReference, getCurrentRPY,
109  getReferencePose, getReferencePosition, getReferenceReference, getReferenceRPY]
110  @param frame_name str: set reference frame name (available from version 315.2.5)
111  @rtype: {str: [float]}
112  @return: Dictionary of the values for each kinematic group.
113  Example (using getCurrentPosition):
114 
115  [{CHEST_JOINT0: [0.0, 0.0, 0.0]},
116  {HEAD_JOINT1: [0.0, 0.0, 0.5694999999999999]},
117  {RARM_JOINT5: [0.3255751238415409, -0.18236314012331808, 0.06762452267747099]},
118  {LARM_JOINT5: [0.3255751238415409, 0.18236314012331808, 0.06762452267747099]}]
119  '''
120  _geometry_methods = ['getCurrentPose', 'getCurrentPosition',
121  'getCurrentReference', 'getCurrentRPY',
122  'getReferencePose', 'getReferencePosition',
123  'getReferenceReference', 'getReferenceRPY']
124  if method.__name__ not in _geometry_methods:
125  raise NameError("Passed method {} is not supported.".format(method))
126  for kinematic_group in self.Groups:
127  # The last element is usually an eef in each kinematic group,
128  # although not required so.
129  eef_name = kinematic_group[1][-1]
130  try:
131  result = method(eef_name, frame_name)
132  except RuntimeError as e:
133  print(str(e))
134  print("{}: {}".format(eef_name, method(eef_name)))
135  raise RuntimeError("Since no link name passed, ran it for all"
136  " available eefs.")
137 
138  def getCurrentPose(self, lname=None, frame_name=None):
139  '''!@brief
140  Returns the current physical pose of the specified joint.
141  cf. getReferencePose that returns commanded value.
142 
143  eg.
144  \verbatim
145  IN: robot.getCurrentPose('LARM_JOINT5')
146  OUT: [-0.0017702356144599085,
147  0.00019034630541264752,
148  -0.9999984150158207,
149  0.32556275164378523,
150  0.00012155879975329215,
151  0.9999999745367515,
152  0.0001901314142046251,
153  0.18236394191140365,
154  0.9999984257434246,
155  -0.00012122202968358842,
156  -0.001770258707652326,
157  0.07462472659364472,
158  0.0,
159  0.0,
160  0.0,
161  1.0]
162  \endverbatim
163 
164  @type lname: str
165  @param lname: Name of the link.
166  @param frame_name str: set reference frame name (from 315.2.5)
167  @rtype: list of float
168  @return: Rotational matrix and the position of the given joint in
169  1-dimensional list, that is:
170  \verbatim
171  [a11, a12, a13, x,
172  a21, a22, a23, y,
173  a31, a32, a33, z,
174  0, 0, 0, 1]
175  \endverbatim
176  '''
177  if not lname:
178  self._get_geometry(self.getReferenceRPY, frame_name)
179 
187  if StrictVersion(self.fk_version) >= StrictVersion('315.2.5'):
188  if self.default_frame_name and frame_name is None:
189  frame_name = self.default_frame_name
190  if frame_name and not ':' in lname:
191  lname = lname + ':' + frame_name
192  else: # hrpsys < 315.2.4
193  if frame_name:
194  print('frame_name ('+lname+') is not supported')
195  pose = self.fk_svc.getCurrentPose(lname)
196  if not pose[0]:
197  raise RuntimeError("Could not find reference : " + lname)
198  return pose[1].data
199 
200  def getReferencePose(self, lname=None, frame_name=None):
201  '''!@brief
202  Returns the current commanded pose of the specified joint.
203  cf. getCurrentPose that returns physical pose.
204 
205  @type lname: str
206  @param lname: Name of the link.
207  @param frame_name str: set reference frame name (from 315.2.5)
208  @rtype: list of float
209  @return: Rotational matrix and the position of the given joint in
210  1-dimensional list, that is:
211  \verbatim
212  [a11, a12, a13, x,
213  a21, a22, a23, y,
214  a31, a32, a33, z,
215  0, 0, 0, 1]
216  \endverbatim
217  '''
218  if not lname:
219  # Requires hrpsys 315.12.1 or higher.
220  self._get_geometry(self.getReferenceRPY, frame_name)
221  if StrictVersion(self.fk_version) >= StrictVersion('315.2.5'):
222  if self.default_frame_name and frame_name is None:
223  frame_name = self.default_frame_name
224  if frame_name and not ':' in lname:
225  lname = lname + ':' + frame_name
226  else: # hrpsys < 315.2.4
227  if frame_name:
228  print('frame_name ('+lname+') is not supported')
229  pose = self.fk_svc.getReferencePose(lname)
230  if not pose[0]:
231  raise RuntimeError("Could not find reference : " + lname)
232  return pose[1].data
233 
234  def setTargetPose(self, gname, pos, rpy, tm, frame_name=None):
235  '''!@brief
236  Move the end-effector to the given absolute pose.
237  All d* arguments are in meter.
238 
239  @param gname str: Name of the joint group. Case-insensitive
240  @param pos list of float: In meter.
241  @param rpy list of float: In radian.
242  @param tm float: Second to complete the command.
243  @param frame_name str: Name of the frame that this particular command
244  references to.
245  @return bool: False if unreachable.
246  '''
247  print(gname, frame_name, pos, rpy, tm)
248  # Same change as https://github.com/fkanehiro/hrpsys-base/pull/1113.
249  # This method should be removed as part of
250  # https://github.com/start-jsk/rtmros_hironx/pull/470, once
251  # https://github.com/fkanehiro/hrpsys-base/pull/1063 resolves.
252  if gname.upper() not in map (lambda x : x[0].upper(), self.Groups):
253  print("setTargetPose failed. {} is not available in the kinematic groups. "
254  "Check available Groups (by e.g. self.Groups/robot.Groups). ".format(gname))
255  return False
256  if StrictVersion(self.seq_version) >= StrictVersion('315.2.5'):
257  if self.default_frame_name and frame_name is None:
258  frame_name = self.default_frame_name
259  if frame_name and not ':' in gname:
260  gname = gname + ':' + frame_name
261  else: # hrpsys < 315.2.4
262  if frame_name and not ':' in gname:
263  print('frame_name ('+gname+') is not supported')
264  result = self.seq_svc.setTargetPose(gname, pos, rpy, tm)
265  if not result:
266  print("setTargetPose failed. Maybe SequencePlayer failed to solve IK.\n"
267  + "Currently, returning IK result error\n"
268  + "(like the one in https://github.com/start-jsk/rtmros_hironx/issues/103)"
269  + " is not implemented. Patch is welcomed.")
270  return result
271 
273  '''
274  @see: <a href = "https://github.com/fkanehiro/hrpsys-base/blob/master/" +
275  "python/hrpsys_config.py">HrpsysConfigurator</a>
276 
277  This class holds methods that are specific to Kawada Industries' dual-arm
278  robot called Hiro.
279 
280  For the API doc for the derived methods, please see the parent
281  class via the link above; nicely formatted api doc web page
282  isn't available yet (discussed in
283  https://github.com/fkanehiro/hrpsys-base/issues/268).
284  '''
285 
286  Groups = [['torso', ['CHEST_JOINT0']],
287  ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']],
288  ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2',
289  'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']],
290  ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2',
291  'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5']]]
292 
293  '''
294  For OffPose and _InitialPose, the angles of each joint are listed in the
295  ordered as defined in Groups variable.'''
296  OffPose = [[0],
297  [0, 0],
298  [25, -139, -157, 45, 0, 0],
299  [-25, -139, -157, -45, 0, 0],
300  [0, 0, 0, 0],
301  [0, 0, 0, 0]]
302  # With this pose the EEFs level up the tabletop surface.
303  _InitialPose = [[0], [0, 0],
304  [-0.6, 0, -100, 15.2, 9.4, 3.2],
305  [0.6, 0, -100, -15.2, 9.4, -3.2],
306  [0, 0, 0, 0],
307  [0, 0, 0, 0]]
308  # This pose sets joint angles at the factory initial pose. No danger, but
309  # no real advange either for in normal usage.
310  # See https://github.com/start-jsk/rtmros_hironx/issues/107
311  _InitialPose_Factory = [[0], [0, 0],
312  [-0, 0, -130, 0, 0, 0],
313  [0, 0, -130, 0, 0, 0],
314  [0, 0, 0, 0],
315  [0, 0, 0, 0]]
316  INITPOS_TYPE_EVEN = 0
317  INITPOS_TYPE_FACTORY = 1
318 
319  HandGroups = {'rhand': [2, 3, 4, 5], 'lhand': [6, 7, 8, 9]}
320 
321  RtcList = []
322 
323  # servo controller (grasper)
324  sc = None
325  sc_svc = None
326 
327  hrpsys_version = '0.0.0'
328 
329  _MSG_IMPEDANCE_CALL_DONE = (" call is done. This does't necessarily mean " +
330  "the function call was successful, since not " +
331  "all methods internally called return status")
332 
333  def init(self, robotname="HiroNX(Robot)0", url=""):
334  '''
335  Calls init from its superclass, which tries to connect RTCManager,
336  looks for ModelLoader, and starts necessary RTC components. Also runs
337  config, logger.
338  Also internally calls setSelfGroups().
339 
340  @type robotname: str
341  @type url: str
342  '''
343  # reload for hrpsys 315.1.8
344  print(self.configurator_name + "waiting ModelLoader")
345  HrpsysConfigurator.waitForModelLoader(self)
346  print(self.configurator_name + "start hrpsys")
347 
348  print(self.configurator_name + "finding RTCManager and RobotHardware")
349  HrpsysConfigurator.waitForRTCManagerAndRoboHardware(self, robotname=robotname)
350  print self.configurator_name, "Hrpsys controller version info: "
351  if self.ms :
352  print self.configurator_name, " ms = ", self.ms
353  if self.ms and self.ms.ref :
354  print self.configurator_name, " ref = ", self.ms.ref
355  if self.ms and self.ms.ref and len(self.ms.ref.get_component_profiles()) > 0:
356  print self.configurator_name, " version = ", self.ms.ref.get_component_profiles()[0].version
357  if self.ms and self.ms.ref and len(self.ms.ref.get_component_profiles()) > 0 and StrictVersion(self.ms.ref.get_component_profiles()[0].version) < StrictVersion('315.2.0'):
358  sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'hrpsys_315_1_9/hrpsys'))
359  delete_module('ImpedanceControllerService_idl')
360  import AbsoluteForceSensorService_idl
361  import ImpedanceControllerService_idl
362 
363  # HrpsysConfigurator.init(self, robotname=robotname, url=url)
364  self.sensors = self.getSensors(url)
365 
366  # all([rtm.findRTC(rn[0], rtm.rootnc) for rn in self.getRTCList()]) # not working somehow...
367  if set([rn[0] for rn in self.getRTCList()]).issubset(set([x.name() for x in self.ms.get_components()])) :
368  print(self.configurator_name + "hrpsys components are already created and running")
369  self.findComps(max_timeout_count=0, verbose=True)
370  else:
371  print(self.configurator_name + "no hrpsys components found running. creating now")
372  self.createComps()
373 
374  print(self.configurator_name + "connecting hrpsys components")
375  self.connectComps()
376 
377  print(self.configurator_name + "activating hrpsys components")
378  self.activateComps()
379 
380  print(self.configurator_name + "setup hrpsys logger")
381  self.setupLogger()
382 
383  print(self.configurator_name + "setup joint groups for hrpsys controller")
384  self.setSelfGroups()
385 
386  print(self.configurator_name + '\033[32minitialized successfully\033[0m')
387 
388  # set hrpsys_version
389  try:
390  self.hrpsys_version = self.fk.ref.get_component_profile().version
391  except:
392  print(self.configurator_name + '\033[34mCould not get hrpsys_version\033[0m')
393 
394  pass
395  self.setSelfGroups()
396  self.hrpsys_version = self.fk.ref.get_component_profile().version
397 
398  def goOffPose(self, tm=7):
399  '''
400  Move arms to the predefined (as member variable) pose where robot can
401  be safely turned off.
402 
403  @type tm: float
404  @param tm: Second to complete.
405  '''
406  for i in range(len(self.Groups)):
407  self.setJointAnglesOfGroup(self.Groups[i][0], self.OffPose[i], tm,
408  wait=False)
409  for i in range(len(self.Groups)):
410  self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
411  self.servoOff(wait=False)
412 
413  def goInitial(self, tm=7, wait=True, init_pose_type=0):
414  '''
415  Move arms to the predefined (as member variable) "initialized" pose.
416 
417  @type tm: float
418  @param tm: Second to complete.
419  @type wait: bool
420  @param wait: If true, other command to the robot's joints wait until
421  this command returns (done by running
422  SequencePlayer.waitInterpolationOfGroup).
423  @type init_pose_type: int
424  @param init_pose_type: 0: default init pose (specified as _InitialPose)
425  1: factory init pose (specified as
426  _InitialPose_Factory)
427  '''
428  if init_pose_type == self.INITPOS_TYPE_FACTORY:
429  _INITPOSE = self._InitialPose_Factory
430  else:
431  _INITPOSE = self._InitialPose
432 
433  ret = True
434  for i in range(len(self.Groups)):
435  # radangles = [x/180.0*math.pi for x in self._InitialPose[i]]
436  print(
437  '{}, JntAnglesOfGr={}, INITPOSE[i]={}, tm={}, wait={}'.format(
438  self.configurator_name, self.Groups[i][0], _INITPOSE[i],
439  tm, wait))
440  ret &= self.setJointAnglesOfGroup(self.Groups[i][0],
441  _INITPOSE[i],
442  tm, wait=False)
443  if wait:
444  for i in range(len(self.Groups)):
445  self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
446  return ret
447 
448  def getRTCList(self):
449  '''
450  @see: HrpsysConfigurator.getRTCList
451 
452  @rtype [[str]]
453  @rerutrn List of available components. Each element consists of a list
454  of abbreviated and full names of the component.
455  '''
456  rtclist = [
457  ['seq', "SequencePlayer"],
458  ['sh', "StateHolder"],
459  ['fk', "ForwardKinematics"],
460  ['ic', "ImpedanceController"],
461  ['el', "SoftErrorLimiter"],
462  ['co', "CollisionDetector"],
463  ['sc', "ServoController"],
464  ['log', "DataLogger"],
465  ]
466 
467  # Want to move the following to upstream:
468 
470  if self.ms and self.ms.ref and len(self.ms.ref.get_component_profiles()) > 0:
471  for rtc in copy.deepcopy(rtclist):
472  try:
473  enable = next(p for p
474  in self.ms.ref.get_component_profiles()[0].properties
475  if p.name == (rtc[1] + '.enable')).value.value()
476  except StopIteration:
477  enable = 'YES'
478  if enable == 'NO':
479  rtclist.remove(rtc)
480  elif enable != 'YES':
481  print(self.configurator_name +
482  '\033[31mConfig "' + (rtc[1] + '.enable') + '" is ' +
483  enable + '. Set YES or NO\033[0m')
484 
485  # Specific code to current HIRONX status:
486 
487  co_rtc = ['co', "CollisionDetector"]
488  if co_rtc not in rtclist:
489  pass
490  elif self.ms and self.ms.ref and len(self.ms.ref.get_component_profiles()) > 0:
491  try:
492  next(p for p
493  in self.ms.ref.get_component_profiles()[0].properties
494  if p.name == (co_rtc[1] + '.enable')).value.value()
495  except StopIteration:
496  rtclist.remove(co_rtc)
497  else:
498  rtclist.remove(co_rtc)
499 
500  if hasattr(self, 'rmfo'):
501  self.ms.load("RemoveForceSensorLinkOffset")
502  self.ms.load("AbsoluteForceSensor")
503  if "RemoveForceSensorLinkOffset" in self.ms.get_factory_names():
504  rtclist.append(['rmfo', "RemoveForceSensorLinkOffset"])
505  elif "AbsoluteForceSensor" in self.ms.get_factory_names():
506  rtclist.append(['rmfo', "AbsoluteForceSensor"])
507  else:
508  print "Component rmfo is not loadable."
509  return rtclist
510 
511  # hand interface
512  # effort: 1~100[%]
513  # hiro.HandOpen("rhand", effort)
514  # hiro.HandOpen() # for both hand
515  # hiro.HandClose("rhand", effort)
516  # hiro.HandClose() # for both hand
517  #
518  def HandOpen(self, hand=None, effort=None):
519  '''
520  Set the stretch between two fingers of the specified hand as
521  hardcoded value (100mm), by internally calling self.setHandWidth.
522 
523  @type hand: str
524  @param hand: Name of the hand joint group. In the default
525  setting of HIRONX, hand joint groups are defined
526  in member 'HandGroups' where 'lhand' and 'rhand'
527  are added.
528  @type effort: int
529  '''
530  self.setHandWidth(hand, 100, effort=effort)
531 
532  def HandClose(self, hand=None, effort=None):
533  '''
534  Close 2-finger hand, by internally calling self.setHandWidth
535  setting 0 width.
536 
537  @type hand: str
538  @param hand: Name of the hand joint group. In the default
539  setting of HIRONX, hand joint groups are defined
540  in member 'HandGroups' where 'lhand' and 'rhand'
541  are added.
542  @type effort: int
543  '''
544  self.setHandWidth(hand, 0, effort=effort)
545 
546  def setHandJointAngles(self, hand, angles, tm=1):
547  '''
548  @type hand: str
549  @param hand: Name of the hand joint group. In the default
550  setting of HIRONX, hand joint groups are defined
551  in member 'HandGroups' where 'lhand' and 'rhand'
552  are added.
553  @type angles: OpenHRP::ServoControllerService::dSequence.
554  @param angles: List of (TODO: document). Elements are in degree.
555  @param tm: Time to complete the task.
556  '''
557  self.sc_svc.setJointAnglesOfGroup(hand, angles, tm)
558 
559  def setHandEffort(self, effort=100):
560  '''
561  Set maximum torque for all existing hand components.
562  @type effort: int
563  '''
564 
565  for i in [v for vs in self.HandGroups.values() for v in vs]: # flatten
566  self.sc_svc.setMaxTorque(i, effort)
567 
568  def setHandWidth(self, hand, width, tm=1, effort=None):
569  '''
570  @type hand: str
571  @param hand: Name of the hand joint group. In the default
572  setting of HIRONX, hand joint groups are defined
573  in member 'HandGroups' where 'lhand' and 'rhand'
574  are added.
575  @param width: Max=100.
576  @param tm: Time to complete the move.
577  @type effort: int
578  @param effort: Passed to self.setHandEffort if set. Not set by default.
579  '''
580  if effort:
581  self.setHandEffort(effort)
582  if hand:
583  self.setHandJointAngles(hand, self.hand_width2angles(width), tm)
584  else:
585  for h in self.HandGroups.keys():
586  self.setHandJointAngles(h, self.hand_width2angles(width), tm)
587 
588  def moveHand(self, hand, av, tm=1): # direction av: + for open, - for close
589  '''
590  Negate the angle value for {2, 3, 6, 7}th element in av.
591 
592  @type hand: str
593  @param hand: Specifies hand. (TODO: List the possible values. Should be
594  listed in setHandJointAngles so just copy from its doc.)
595  @type av: [int]
596  @param av: angle of each joint of the specified arm
597  (TODO: need verified. Also what's the length of the list?)
598  @param tm: Time in second to complete the work.
599  '''
600  for i in [2, 3, 6, 7]: # do not change this line if servo is different, change HandGroups
601  av[i] = -av[i]
602  self.setHandJointAngles(hand, av, tm)
603 
604  def hand_width2angles(self, width):
605  '''
606  TODO: Needs documented what this method does.
607 
608  @type width: float
609  @return: None if the given width is invalid.
610  '''
611  safetyMargin = 3
612  l1, l2 = (41.9, 19) # TODO: What are these variables?
613 
614  if width < 0.0 or width > (l1 + l2 - safetyMargin) * 2:
615  return None
616 
617  xPos = width / 2.0 + safetyMargin
618  a2Pos = xPos - l2
619  a1radH = math.acos(a2Pos / l1)
620  a1rad = math.pi / 2.0 - a1radH
621 
622  return a1rad, -a1rad, -a1rad, a1rad
623 
624  def setSelfGroups(self):
625  '''
626  Set to the hrpsys.SequencePlayer the groups of links and joints that
627  are statically defined as member variables (Groups) within this class.
628 
629  That said, override Groups variable if you prefer link and joint
630  groups set differently.
631  '''
632  #TODO: Accept groups variable. The name of the method sounds more
633  # natural if it accepts it.
634  for item in self.Groups:
635  self.seq_svc.addJointGroup(item[0], item[1])
636  for k, v in self.HandGroups.iteritems():
637  if self.sc_svc:
638  self.sc_svc.addJointGroup(k, v)
639 
640  def isCalibDone(self):
641  '''
642  Check whether joints have been calibrated.
643  @rtype bool
644  '''
645  if self.simulation_mode:
646  return True
647  else:
648  rstt = self.rh_svc.getStatus()
649  for item in rstt.servoState:
650  if not item[0] & 1:
651  return False
652  return True
653 
654  def isServoOn(self, jname='any'):
655  '''
656  Check whether servo control has been turned on. Check is done by
657  HIRONX.getActualState().servoState.
658  @type jname: str
659  @param jname: Name of a link (that can be obtained by "hiro.Groups"
660  as lists of groups).
661 
662  Reserved values:
663  - any: This command will check all servos available.
664  - all: Same as 'any'.
665  @rtype bool
666  @return: If jname is specified either 'any' or 'all', return False
667  if the control of any of servos isn't available.
668  '''
669  if self.simulation_mode:
670  return True
671  else:
672  s_s = self.getActualState().servoState
673  if jname.lower() == 'any' or jname.lower() == 'all':
674  for s in s_s:
675  # print self.configurator_name, 's = ', s
676  if (s[0] & 2) == 0:
677  return False
678  else:
679  jid = eval('self.' + jname)
680  print self.configurator_name, s_s[jid]
681  if s_s[jid][0] & 1 == 0:
682  return False
683  return True
684  return False
685 
686  def flat2Groups(self, flatList):
687  '''
688  @type flatList: [int]
689  @param flatList: single dimension list, with its length depends on
690  'Groups' variable defined within this class. Excessive
691  elements will be dropped (see example below in @return)
692 
693  eg. If the number of joints of the robot is 15,
694  len(flatList) should be 15.
695  @rtype: [[]]
696  @return: 2-dimensional list that has the same format with
697  'Groups' variable.
698 
699  eg.
700  ipython> flatlist = [0, 10, 20, 30, 40, 50, 60, 70, 80, 90,
701  100, 110, 120, 130, 140, 150]
702  ipython> robot.flat2Groups(flatlist)
703  [[0], [10, 20],
704  [30, 40, 50, 60, 70, 80],
705  [90, 100, 110, 120, 130, 140]]
706 
707  '''
708  retList = []
709  index = 0
710  for group in self.Groups:
711  joint_num = len(group[1])
712  retList.append(flatList[index: index + joint_num])
713  index += joint_num
714  return retList
715 
716  def servoOn(self, jname='all', destroy=1, tm=3):
717  '''
718  Turn on servo motors at joint specified.
719  Joints need to be calibrated (otherwise error returns).
720 
721  *Troubleshooting*
722  When this method does not seem to function as expected, try the
723  following first before you report to the developer's community:
724 
725  - Manually move the arms to the safe pose where arms do not obstruct
726  to anything and they can move back to the initial pose by goInitial.
727  Then run the command again.
728  - Make sure the emergency switch is toggled back.
729  - Try running goActual() then servoOn().
730 
731  If none of the above did not solve your issue, please report with:
732  - The result of this command (%ROSDISTRO% is "indigo" as of May 2017):
733 
734  Ubuntu$ rosversion hironx_ros_bridge
735  Ubuntu$ dpkg -p ros-%ROSDISTRO%-hironx-ros-bridge
736 
737  @type jname: str
738  @param jname: The value 'all' works iteratively for all servos.
739  @param destroy: Not used.
740  @type tm: float
741  @param tm: Second to complete.
742  @rtype: int
743  @return: 1 or -1 indicating success or failure, respectively.
744  '''
745  # check joints are calibrated
746  if not self.isCalibDone():
747  waitInputConfirm('!! Calibrate Encoders with checkEncoders first !!\n\n')
748  return -1
749 
750  # check servo state
751  if self.isServoOn():
752  return 1
753 
754  # check jname is acceptable
755  if jname == '':
756  jname = 'all'
757 
758  #self.rh_svc.power('all', SWITCH_ON) #do not switch on before goActual
759 
760  try:
761  waitInputConfirm(
762  '!! Robot Motion Warning (SERVO_ON) !!\n\n'
763  'Confirm RELAY switched ON\n'
764  'Push [OK] to switch servo ON(%s).' % (jname))
765  except: # ths needs to change
766  self.rh_svc.power('all', SWITCH_OFF)
767  raise
768 
769  # Need to reset JointGroups.
770  # See https://code.google.com/p/rtm-ros-robotics/issues/detail?id=277
771  try:
772  # remove jointGroups
773  self.seq_svc.removeJointGroup("larm")
774  self.seq_svc.removeJointGroup("rarm")
775  self.seq_svc.removeJointGroup("head")
776  self.seq_svc.removeJointGroup("torso")
777  except:
778  print(self.configurator_name,
779  'Exception during servoOn while removing JoingGroup. ' +
780  _MSG_ASK_ISSUEREPORT)
781  try:
782  # setup jointGroups
783  self.setSelfGroups() # restart groups
784  except:
785  print(self.configurator_name,
786  'Exception during servoOn while removing setSelfGroups. ' +
787  _MSG_ASK_ISSUEREPORT)
788 
789  try:
790  self.goActual() # This needs to happen before turning servo on.
791  time.sleep(0.1)
792  self.rh_svc.servo(jname, SWITCH_ON)
793  time.sleep(2)
794  # time.sleep(7)
795  if not self.isServoOn(jname):
796  print self.configurator_name, 'servo on failed.'
797  raise
798  except:
799  print self.configurator_name, 'exception occured'
800 
801  try:
802  angles = self.flat2Groups(map(numpy.rad2deg,
803  self.getActualState().angle))
804  print 'Move to Actual State, Just a minute.'
805  for i in range(len(self.Groups)):
806  self.setJointAnglesOfGroup(self.Groups[i][0], angles[i], tm,
807  wait=False)
808  for i in range(len(self.Groups)):
809  self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
810  except:
811  print self.configurator_name, 'post servo on motion trouble'
812 
813  # turn on hand motors
814  print 'Turn on Hand Servo'
815  if self.sc_svc:
816  is_servoon = self.sc_svc.servoOn()
817  print('Hands Servo on: ' + str(is_servoon))
818  if not is_servoon:
819  print('One or more hand servos failed to turn on. Make sure all hand modules are properly cabled ('
820  + _MSG_RESTART_QNX + ') and run the command again.')
821  return -1
822  else:
823  print('hrpsys ServoController not found. Ignore this if you' +
824  ' do not intend to use hand servo (e.g. NEXTAGE Open).' +
825  ' If you do intend, then' + _MSG_RESTART_QNX +
826  ' and run the command again.')
827 
828  return 1
829 
830  def servoOff(self, jname='all', wait=True):
831  '''
832  @type jname: str
833  @param jname: The value 'all' works iteratively for all servos.
834  @type wait: bool
835  @rtype: int
836  @return: 1 = all arm servo off. 2 = all servo on arms and hands off.
837  -1 = Something wrong happened.
838  '''
839  # do nothing for simulation
840  if self.simulation_mode:
841  print self.configurator_name, 'omit servo off'
842  return 0
843 
844  print 'Turn off Hand Servo'
845  if self.sc_svc:
846  self.sc_svc.servoOff()
847  # if the servos aren't on switch power off
848  if not self.isServoOn(jname):
849  if jname.lower() == 'all':
850  self.rh_svc.power('all', SWITCH_OFF)
851  return 1
852 
853  # if jname is not set properly set to all -> is this safe?
854  if jname == '':
855  jname = 'all'
856 
857  if wait:
858  waitInputConfirm(
859  '!! Robot Motion Warning (Servo OFF)!!\n\n'
860  'Push [OK] to servo OFF (%s).' % (jname)) # :
861 
862  try:
863  self.rh_svc.servo('all', SWITCH_OFF)
864  time.sleep(0.2)
865  if jname == 'all':
866  self.rh_svc.power('all', SWITCH_OFF)
867 
868  # turn off hand motors
869  print 'Turn off Hand Servo'
870  if self.sc_svc:
871  self.sc_svc.servoOff()
872 
873  return 2
874  except:
875  print self.configurator_name, 'servo off: communication error'
876  return -1
877 
878  def checkEncoders(self, jname='all', option=''):
879  '''
880  Run the encoder checking sequence for specified joints,
881  run goActual to adjust the direction values, and then turn servos on.
882 
883  @type jname: str
884  @param jname: The value 'all' works iteratively for all servos.
885  @type option: str
886  @param option: Possible values are follows (w/o double quote):\
887  "-overwrite": Overwrite calibration value.
888  '''
889  if self.isServoOn():
890  waitInputConfirm('Servo must be off for calibration')
891  return
892  # do not check encoders twice
893  elif self.isCalibDone():
894  waitInputConfirm('System has been calibrated')
895  return
896 
897  self.rh_svc.power('all', SWITCH_ON)
898  msg = '!! Robot Motion Warning !!\n'\
899  'Turn Relay ON.\n'\
900  'Then Push [OK] to '
901  if option == '-overwrite':
902  msg = msg + 'calibrate(OVERWRITE MODE) '
903  else:
904  msg = msg + 'check '
905 
906  if jname == 'all':
907  msg = msg + 'the Encoders of all.'
908  else:
909  msg = msg + 'the Encoder of the Joint "' + jname + '".'
910 
911  try:
912  waitInputConfirm(msg)
913  except:
914  print "If you're connecting to the robot from remote, " + \
915  "make sure tunnel X (eg. -X option with ssh)."
916  self.rh_svc.power('all', SWITCH_OFF)
917  return 0
918 
919  is_result_hw = True
920  print self.configurator_name, 'calib-joint ' + jname + ' ' + option
921  self.rh_svc.initializeJointAngle(jname, option)
922  print self.configurator_name, 'done'
923  is_result_hw = is_result_hw and self.rh_svc.power('all', SWITCH_OFF)
924  self.goActual() # This needs to happen before turning servo on.
925  time.sleep(0.1)
926  is_result_hw = is_result_hw and self.rh_svc.servo(jname, SWITCH_ON)
927  if not is_result_hw:
928  # The step described in the following msg is confirmed by the manufacturer 12/14/2015
929  print("Turning servos ({}) failed. This is likely because of issues happening in lower level. Please check if the Kawada's proprietary tool NextageOpenSupervisor returns without issue or not. If the issue persists, contact the manufacturer.".format(jname))
930  # turn on hand motors
931  print 'Turn on Hand Servo'
932  if self.sc_svc:
933  self.sc_svc.servoOn()
934 
935  def startImpedance_315_1(self, arm,
936  M_p = 100.0,
937  D_p = 100.0,
938  K_p = 100.0,
939  M_r = 100.0,
940  D_r = 2000.0,
941  K_r = 2000.0,
942  ref_force = [0, 0, 0],
943  force_gain = [1, 1, 1],
944  ref_moment = [0, 0, 0],
945  moment_gain = [0, 0, 0],
946  sr_gain = 1.0,
947  avoid_gain = 0.0,
948  reference_gain = 0.0,
949  manipulability_limit = 0.1):
950  '''
951  @type arm: str name of artm to be controlled, this must be initialized
952  using setSelfGroups()
953  @param ref_{force, moment}: Target values at the target position.
954  Units: N, Nm, respectively.
955  @param {force, moment}_gain: multipliers to the eef offset position
956  vel_p and orientation vel_r. 3-dimensional
957  vector (then converted internally into a
958  diagonal matrix).
959  '''
960  ic_sensor_name = 'rhsensor'
961  ic_target_name = 'RARM_JOINT5'
962  if arm == 'rarm':
963  ic_sensor_name = 'rhsensor'
964  ic_target_name = 'RARM_JOINT5'
965  elif arm == 'larm':
966  ic_sensor_name = 'lhsensor'
967  ic_target_name = 'LARM_JOINT5'
968  else:
969  print 'startImpedance: argument must be rarm or larm.'
970  return
971 
972  self.ic_svc.setImpedanceControllerParam(
973  OpenHRP.ImpedanceControllerService.impedanceParam(
974  name = ic_sensor_name,
975  base_name = 'CHEST_JOINT0',
976  target_name = ic_target_name,
977  M_p = M_p,
978  D_p = D_p,
979  K_p = K_p,
980  M_r = M_r,
981  D_r = D_r,
982  K_r = K_r,
983  ref_force = ref_force,
984  force_gain = force_gain,
985  ref_moment = ref_moment,
986  moment_gain = moment_gain,
987  sr_gain = sr_gain,
988  avoid_gain = avoid_gain,
989  reference_gain = reference_gain,
990  manipulability_limit = manipulability_limit))
991 
992  def stopImpedance_315_1(self, arm):
993  ic_sensor_name = 'rhsensor'
994  if arm == 'rarm':
995  ic_sensor_name = 'rhsensor'
996  elif arm == 'larm':
997  ic_sensor_name = 'lhsensor'
998  else:
999  print 'startImpedance: argument must be rarm or larm.'
1000  return
1001  self.ic_svc.deleteImpedanceControllerAndWait(ic_sensor_name)
1002 
1003  def startImpedance_315_2(self, arm,
1004  M_p = 100.0,
1005  D_p = 100.0,
1006  K_p = 100.0,
1007  M_r = 100.0,
1008  D_r = 2000.0,
1009  K_r = 2000.0,
1010  force_gain = [1, 1, 1],
1011  moment_gain = [0, 0, 0],
1012  sr_gain = 1.0,
1013  avoid_gain = 0.0,
1014  reference_gain = 0.0,
1015  manipulability_limit = 0.1):
1016  '''
1017  @type arm: str name of artm to be controlled, this must be initialized
1018  using setSelfGroups()
1019  @param {force, moment}_gain: multipliers to the eef offset position
1020  vel_p and orientation vel_r. 3-dimensional
1021  vector (then converted internally into a
1022  diagonal matrix).
1023  '''
1024  self.ic_svc.setImpedanceControllerParam(
1025  arm,
1026  OpenHRP.ImpedanceControllerService.impedanceParam(
1027  M_p = M_p,
1028  D_p = D_p,
1029  K_p = K_p,
1030  M_r = M_r,
1031  D_r = D_r,
1032  K_r = K_r,
1033  force_gain = force_gain,
1034  moment_gain = moment_gain,
1035  sr_gain = sr_gain,
1036  avoid_gain = avoid_gain,
1037  reference_gain = reference_gain,
1038  manipulability_limit = manipulability_limit))
1039  return self.ic_svc.startImpedanceController(arm)
1040 
1041  def startImpedance_315_3(self, arm,
1042  M_p = 100.0,
1043  D_p = 100.0,
1044  K_p = 100.0,
1045  M_r = 100.0,
1046  D_r = 2000.0,
1047  K_r = 2000.0,
1048  force_gain = [1, 1, 1],
1049  moment_gain = [0, 0, 0],
1050  sr_gain = 1.0,
1051  avoid_gain = 0.0,
1052  reference_gain = 0.0,
1053  manipulability_limit = 0.1):
1054  '''
1055  @type arm: str name of artm to be controlled, this must be initialized
1056  using setSelfGroups()
1057  @param {force, moment}_gain: multipliers to the eef offset position
1058  vel_p and orientation vel_r. 3-dimensional
1059  vector (then converted internally into a
1060  diagonal matrix).
1061  '''
1062  r, p = self.ic_svc.getImpedanceControllerParam(arm)
1063  if not r:
1064  print('{}, impedance parameter not found for {}.'.format(self.configurator_name, arm))
1065  return False
1066  if M_p != None: p.M_p = M_p
1067  if D_p != None: p.M_p = D_p
1068  if K_p != None: p.M_p = K_p
1069  if M_r != None: p.M_r = M_r
1070  if D_r != None: p.M_r = D_r
1071  if K_r != None: p.M_r = K_r
1072  if force_gain != None: p.force_gain = force_gain
1073  if moment_gain != None: p.moment_gain = moment_gain
1074  if sr_gain != None: p.sr_gain = sr_gain
1075  if avoid_gain != None: p.avoid_gain = avoid_gain
1076  if reference_gain != None: p.reference_gain = reference_gain
1077  if manipulability_limit != None: p.manipulability_limit = manipulability_limit
1078  self.ic_svc.setImpedanceControllerParam(arm, p)
1079  return self.ic_svc.startImpedanceController(arm)
1080 
1081  def stopImpedance_315_2(self, arm):
1082  return self.ic_svc.stopImpedanceController(arm)
1083 
1084  def stopImpedance_315_3(self, arm):
1085  return self.ic_svc.stopImpedanceController(arm)
1086 
1087  def startImpedance(self, arm, **kwargs):
1088  '''
1089  Enable the ImpedanceController RT component.
1090  This method internally calls startImpedance-*, hrpsys version-specific
1091  method.
1092 
1093  @requires: ImpedanceController RTC to be activated on the robot's
1094  controller.
1095  @param arm: Name of the kinematic group (i.e. self.Groups[n][0]).
1096  @param kwargs: This varies depending on the version of hrpsys your
1097  robot's controller runs on
1098  (which you can find by "self.hrpsys_version" command).
1099  For instance, if your hrpsys is 315.10.1, refer to
1100  "startImpedance_315_4" method.
1101  @change: (NOTE: This "change" block is a duplicate with the PR in the
1102  upstream https://github.com/fkanehiro/hrpsys-base/pull/1120.
1103  Once it gets merged this block should be removed to avoid
1104  duplicate maintenance effort.)
1105 
1106  From 315.2.0 onward, following arguments are dropped and can
1107  be set by self.seq_svc.setWrenches instead of this method.
1108  See an example at https://github.com/fkanehiro/hrpsys-base/pull/434/files#diff-6204f002204dd9ae80f203901f155fa9R44:
1109  - ref_force[fx, fy, fz] (unit: N) and
1110  ref_moment[tx, ty, tz] (unit: Nm) can be set via
1111  self.seq_svc.setWrenches. For example:
1112 
1113  self.seq_svc.setWrenches([0, 0, 0, 0, 0, 0,
1114  fx, fy, fz, tx, ty, tz,
1115  0, 0, 0, 0, 0, 0,
1116  0, 0, 0, 0, 0, 0,])
1117 
1118  setWrenches takes 6 values per sensor, so the robot in
1119  the example above has 4 sensors where each line represents
1120  a sensor. See this link (https://github.com/fkanehiro/hrpsys-base/pull/434/files) for a concrete example.
1121  '''
1122  if StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'):
1123  self.startImpedance_315_1(arm, **kwargs)
1124  elif StrictVersion(self.hrpsys_version) < StrictVersion('315.3.0'):
1125  self.startImpedance_315_2(arm, **kwargs)
1126  else:
1127  self.startImpedance_315_3(arm, **kwargs)
1128  print('startImpedance {}'.format(self._MSG_IMPEDANCE_CALL_DONE))
1129 
1130  def stopImpedance(self, arm):
1131  if StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'):
1132  self.stopImpedance_315_1(arm)
1133  elif StrictVersion(self.hrpsys_version) < StrictVersion('315.3.0'):
1134  self.stopImpedance_315_2(arm)
1135  else:
1136  self.stopImpedance_315_3(arm)
1137  print('stopImpedance {}'.format(self._MSG_IMPEDANCE_CALL_DONE))
1138 
1140  self.rh_svc.removeForceSensorOffset()
1141 
1142 
1143  # workaround for issue reported https://github.com/tork-a/rtmros_nextage/issues/332
1144  def getJointAnglesOfGroup(self, limb):
1145  angles = self.getJointAngles()
1146  # fix flag2groups for hironx(sim), which have gripper joints
1147  # flat2Groups: requres 315.2.0
1148  # angles = self.flat2Groups(angles)
1149  offset = 0
1150  if len(angles) != reduce(lambda x,y: x+len(y[1]), self.Groups, 0):
1151  offset = 4
1152  angles = []
1153  index = 0
1154  for group in self.Groups:
1155  joint_num = len(group[1])
1156  angles.append(angles[index: index + joint_num])
1157  index += joint_num
1158  if group[0] in ['larm', 'rarm']:
1159  index += offset
1160  groups = self.Groups
1161  for i in range(len(groups)):
1162  if groups[i][0] == limb:
1163  return angles[i]
1164  print self.configurator_name, 'could not find limb name ' + limb
1165  print self.configurator_name, ' in' + filter(lambda x: x[0], groups)
1166 
1167  def clearOfGroup(self, limb):
1168  '''!@brief
1169  Clears the Sequencer's current operation for joint groups.
1170  @since 315.5.0
1171  '''
1172  if StrictVersion(self.seq_version) < StrictVersion('315.5.0'):
1173  raise RuntimeError('clearOfGroup is not available with your '
1174  'software version ' + self.seq_version)
1175  HrpsysConfigurator.clearOfGroup(self, limb)
1176  angles = self.getJointAnglesOfGroup(limb)
1177  print self.configurator_name, 'clearOfGroup(' + limb + ') will send ' + str(angles) + ' to update seqplay until https://github.com/fkanehiro/hrpsys-base/pull/1141 is available'
1178  self.setJointAnglesOfGroup(limb, angles, 0.1, wait=True)
def goInitial(self, tm=7, wait=True, init_pose_type=0)
def init(self, robotname="HiroNX(Robot)0", url="")
def getReferencePose(self, lname=None, frame_name=None)
Returns the current commanded pose of the specified joint.
def clearOfGroup(self, limb)
Clears the Sequencer&#39;s current operation for joint groups.
def setTargetPose(self, gname, pos, rpy, tm, frame_name=None)
Move the end-effector to the given absolute pose.
def setHandJointAngles(self, hand, angles, tm=1)
def servoOff(self, jname='all', wait=True)
def startImpedance_315_2(self, arm, M_p=100.0, D_p=100.0, K_p=100.0, M_r=100.0, D_r=2000.0, K_r=2000.0, force_gain=[1, moment_gain=[0, sr_gain=1.0, avoid_gain=0.0, reference_gain=0.0, manipulability_limit=0.1)
def HandClose(self, hand=None, effort=None)
def getCurrentPose(self, lname=None, frame_name=None)
Returns the current physical pose of the specified joint.
def startImpedance_315_1(self, arm, M_p=100.0, D_p=100.0, K_p=100.0, M_r=100.0, D_r=2000.0, K_r=2000.0, ref_force=[0, force_gain=[1, ref_moment=[0, moment_gain=[0, sr_gain=1.0, avoid_gain=0.0, reference_gain=0.0, manipulability_limit=0.1)
def delete_module(modname, paranoid=None)
def servoOn(self, jname='all', destroy=1, tm=3)
def HandOpen(self, hand=None, effort=None)
def startImpedance_315_3(self, arm, M_p=100.0, D_p=100.0, K_p=100.0, M_r=100.0, D_r=2000.0, K_r=2000.0, force_gain=[1, moment_gain=[0, sr_gain=1.0, avoid_gain=0.0, reference_gain=0.0, manipulability_limit=0.1)
int initializeJointAngle(const char *name, const char *option)
Definition: iob.cpp:681
def _get_geometry(self, method, frame_name=None)
A method only inteded for class-internal usage.
def moveHand(self, hand, av, tm=1)
def checkEncoders(self, jname='all', option='')
def setHandWidth(self, hand, width, tm=1, effort=None)


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