Merge pull request #487 from 130s/fix/getRefPose_arg

[fix][Python] link name's default value for get* methods.

hironx_moveit_config/package.xml

@@ -5,11 +5,11 @@
   <description>
      An automatically generated package with all the configuration and launch files for using the HiroNX with the MoveIt Motion Planning Framework
   </description>
-  <author email="[email protected]">MoveIt Setup Assistant</author>
   <author email="[email protected]">Urko Esnaola</author>
+  <author email="[email protected]">Isaac I.Y. Saito</author>
 
   <maintainer email="[email protected]">Kei Okada</maintainer>
-  <maintainer email="iisaito@opensource-robotics.tokyo.jp">Isaac Isao Saito</maintainer>
+  <maintainer email="dev@opensource-robotics.tokyo.jp">TORK</maintainer>
 
   <license>BSD</license>
 

hironx_ros_bridge/package.xml

@@ -9,8 +9,9 @@
   </description>
 
   <maintainer email="[email protected]">Kei Okada</maintainer>
-  <maintainer email="[email protected]">Isaac Isao Saito</maintainer>
+  <maintainer email="[email protected]">TORK</maintainer>
   <author email="[email protected]">Kei Okada</author>
+  <author email="[email protected]">Isaac I.Y. Saito</author>
 
   <license>BSD</license>
   <license>CreativeCommons-by-nc-4.0</license>

hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py

@@ -87,6 +87,43 @@ def delete_module(modname, paranoid=None):
 class HrpsysConfigurator2(HrpsysConfigurator): ## JUST FOR TEST, REMOVE WHEN YOU MERGE
     default_frame_name = 'WAIST'
 
+    def _get_geometry(self, method, frame_name=None):
+        '''!@brief
+        A method only inteded for class-internal usage.
+
+        @since: 315.12.1 or higher
+        @type method: A Python function object.
+        @param method: One of the following Python function objects defined in class HrpsysConfigurator:
+            [getCurrentPose, getCurrentPosition, getCurrentReference, getCurrentRPY,
+             getReferencePose, getReferencePosition, getReferenceReference, getReferenceRPY]
+        @param frame_name str: set reference frame name (available from version 315.2.5)
+        @rtype: {str: [float]}
+        @return: Dictionary of the values for each kinematic group.
+            Example (using getCurrentPosition):
+
+                [{CHEST_JOINT0: [0.0, 0.0, 0.0]},
+                 {HEAD_JOINT1: [0.0, 0.0, 0.5694999999999999]},
+                 {RARM_JOINT5: [0.3255751238415409, -0.18236314012331808, 0.06762452267747099]},
+                 {LARM_JOINT5: [0.3255751238415409, 0.18236314012331808, 0.06762452267747099]}]
+        '''
+        _geometry_methods = ['getCurrentPose', 'getCurrentPosition',
+                             'getCurrentReference', 'getCurrentRPY',
+                             'getReferencePose', 'getReferencePosition',
+                             'getReferenceReference', 'getReferenceRPY']
+        if method not in _geometry_methods:
+            raise RuntimeError("Passed method {} is not supported.".format(method))
+        for kinematic_group in self.Groups:
+            # The last element is usually an eef in each kinematic group,
+            # although not required so.
+            eef_name = kinematic_group[1][-1]
+            try:
+                result = method(eef_name, frame_name)
+            except RuntimeError as e:
+                print(str(e))
+            print("{}: {}".format(eef_name, method(eef_name)))
+        raise RuntimeError("Since no link name passed, ran it for all"
+                           " available eefs.")
+
     def getCurrentPose(self, lname=None, frame_name=None):
         '''!@brief
         Returns the current physical pose of the specified joint.
@@ -127,10 +164,7 @@ def getCurrentPose(self, lname=None, frame_name=None):
         \endverbatim
         '''
         if not lname:
-            for item in self.Groups:
-                eef_name = item[1][-1]
-                print("{}: {}".format(eef_name, self.getCurrentPose(eef_name)))
-            raise RuntimeError("need to specify joint name")
+            self._get_geometry(self.getReferenceRPY, frame_name)
         ####
         #### for hrpsys >= 315.2.5, frame_name is supported
         ####   default_frame_name is set, call with lname:default_frame_name
@@ -152,7 +186,7 @@ def getCurrentPose(self, lname=None, frame_name=None):
             raise RuntimeError("Could not find reference : " + lname)
         return pose[1].data
 
-    def getReferencePose(self, lname, frame_name=None):
+    def getReferencePose(self, lname=None, frame_name=None):
         '''!@brief
         Returns the current commanded pose of the specified joint.
         cf. getCurrentPose that returns physical pose.
@@ -171,10 +205,8 @@ def getReferencePose(self, lname, frame_name=None):
         \endverbatim
         '''
         if not lname:
-            for item in self.Groups:
-                eef_name = item[1][-1]
-                print("{}: {}".format(eef_name, self.getReferencePose(eef_name)))
-            raise RuntimeError("need to specify joint name")
+            # Requires hrpsys 315.12.1 or higher.
+            self._get_geometry(self.getReferenceRPY, frame_name)
         if StrictVersion(self.fk_version) >= StrictVersion('315.2.5'):                         ### CHANGED
             if self.default_frame_name and frame_name is None:
                 frame_name = self.default_frame_name

rtmros_hironx/package.xml

@@ -8,8 +8,9 @@
   </description> 
 
   <maintainer email="[email protected]">Kei Okada</maintainer>
-  <maintainer email="[email protected]">Isaac IY Saito</maintainer>
+  <maintainer email="[email protected]">TORK</maintainer>
   <author email="[email protected]">Kei Okada</author>
+  <author email="[email protected]">Isaac I.Y. Saito</author>
 
   <license>BSD</license>
   <url type="website">http://ros.org/wiki/rtmros_hironx/</url>
@@ -25,4 +26,4 @@
   <export>
     <metapackage/>
   </export>
-</package>
+</package>