From a40acc27549d8cb409188d689b9d8c76f44f5ae4 Mon Sep 17 00:00:00 2001 From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp> Date: Thu, 13 Oct 2016 08:50:55 +0900 Subject: [PATCH 1/2] [hironx client] Remove experimental dependency (see https://github.com/start-jsk/rtmros_hironx/pull/469#issuecomment-252386697). --- .../src/hironx_ros_bridge/hironx_client.py | 137 +----------------- 1 file changed, 2 insertions(+), 135 deletions(-) diff --git a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py index 5471469e..d55b0f6c 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +++ b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py @@ -84,141 +84,8 @@ def delete_module(modname, paranoid=None): except AttributeError: pass -class HrpsysConfigurator2(HrpsysConfigurator): ## JUST FOR TEST, REMOVE WHEN YOU MERGE - default_frame_name = 'WAIST' - - def getCurrentPose(self, lname=None, frame_name=None): - '''!@brief - Returns the current physical pose of the specified joint. - cf. getReferencePose that returns commanded value. - - eg. - \verbatim - IN: robot.getCurrentPose('LARM_JOINT5') - OUT: [-0.0017702356144599085, - 0.00019034630541264752, - -0.9999984150158207, - 0.32556275164378523, - 0.00012155879975329215, - 0.9999999745367515, - 0.0001901314142046251, - 0.18236394191140365, - 0.9999984257434246, - -0.00012122202968358842, - -0.001770258707652326, - 0.07462472659364472, - 0.0, - 0.0, - 0.0, - 1.0] - \endverbatim - - @type lname: str - @param lname: Name of the link. - @param frame_name str: set reference frame name (from 315.2.5) - @rtype: list of float - @return: Rotational matrix and the position of the given joint in - 1-dimensional list, that is: - \verbatim - [a11, a12, a13, x, - a21, a22, a23, y, - a31, a32, a33, z, - 0, 0, 0, 1] - \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") - #### - #### for hrpsys >= 315.2.5, frame_name is supported - #### default_frame_name is set, call with lname:default_frame_name - #### frame_name is given, call with lname:frame_name - #### frame_name is none, call with lname - #### for hrpsys <= 315.2.5, frame_name is not supported - #### frame_name is given, call with lname with warning / oerror - #### frame_name is none, call with lname - 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 - if frame_name and not ':' in lname: - lname = lname + ':' + frame_name - else: # hrpsys < 315.2.4 - if frame_name: - print('frame_name ('+lname+') is not supported') ### CHANGED - pose = self.fk_svc.getCurrentPose(lname) - if not pose[0]: - raise RuntimeError("Could not find reference : " + lname) - return pose[1].data - - def getReferencePose(self, lname, frame_name=None): - '''!@brief - Returns the current commanded pose of the specified joint. - cf. getCurrentPose that returns physical pose. - - @type lname: str - @param lname: Name of the link. - @param frame_name str: set reference frame name (from 315.2.5) - @rtype: list of float - @return: Rotational matrix and the position of the given joint in - 1-dimensional list, that is: - \verbatim - [a11, a12, a13, x, - a21, a22, a23, y, - a31, a32, a33, z, - 0, 0, 0, 1] - \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") - 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 - if frame_name and not ':' in lname: - lname = lname + ':' + frame_name - else: # hrpsys < 315.2.4 - if frame_name: - print('frame_name ('+lname+') is not supported') ### CHANGED - pose = self.fk_svc.getReferencePose(lname) - if not pose[0]: - raise RuntimeError("Could not find reference : " + lname) - return pose[1].data - - def setTargetPose(self, gname, pos, rpy, tm, frame_name=None): - '''!@brief - Move the end-effector to the given absolute pose. - All d* arguments are in meter. - - @param gname str: Name of the joint group. - @param pos list of float: In meter. - @param rpy list of float: In radian. - @param tm float: Second to complete the command. - @param frame_name str: Name of the frame that this particular command - references to. - @return bool: False if unreachable. - ''' - print(gname, frame_name, pos, rpy, tm) - if StrictVersion(self.seq_version) >= StrictVersion('315.2.5'): ### CHANGED - if self.default_frame_name and frame_name is None: - frame_name = self.default_frame_name - if frame_name and not ':' in gname: - gname = gname + ':' + frame_name - else: # hrpsys < 315.2.4 - if frame_name and not ':' in gname: - print('frame_name ('+gname+') is not supported') ### CHANGED - result = self.seq_svc.setTargetPose(gname, pos, rpy, tm) - if not result: - print("setTargetPose failed. Maybe SequencePlayer failed to solve IK.\n" - + "Currently, returning IK result error\n" - + "(like the one in https://github.com/start-jsk/rtmros_hironx/issues/103)" - + " is not implemented. Patch is welcomed.") - return result - -class HIRONX(HrpsysConfigurator2): + +class HIRONX(HrpsysConfigurator): ''' @see: HrpsysConfigurator From adc378e62b80663e2d4735cf758f963c11cc161c Mon Sep 17 00:00:00 2001 From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp> Date: Sat, 15 Oct 2016 11:39:19 +0900 Subject: [PATCH 2/2] [test] Fix local var referenced before assignment issue (see https://travis-ci.org/start-jsk/rtmros_hironx/jobs/167215999#L7552). --- hironx_ros_bridge/test/test_hironx_target.py | 1 + 1 file changed, 1 insertion(+) diff --git a/hironx_ros_bridge/test/test_hironx_target.py b/hironx_ros_bridge/test/test_hironx_target.py index 9f5117f9..9f43cdcc 100755 --- a/hironx_ros_bridge/test/test_hironx_target.py +++ b/hironx_ros_bridge/test/test_hironx_target.py @@ -174,6 +174,7 @@ def print_pose(msg, pose): self.robot.goInitial() posel1 = self.robot.getCurrentPose('LARM_JOINT5') + posel2 = None try: posel2 = self.robot.getCurrentPose('LARM_JOINT5', 'WAIST') except RuntimeError as e: