From cc2b65a36427bc7f1b97ec73e062995791ce2c9d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 24 Jul 2018 21:34:04 +0900 Subject: [PATCH] add test for getJointAnglesOfGroup --- .../src/hironx_ros_bridge/hironx_client.py | 18 +++++++++++++++--- hironx_ros_bridge/test/test_hironx_limb.py | 10 ++++++++++ 2 files changed, 25 insertions(+), 3 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 8fb58583..c5a39ad2 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +++ b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py @@ -1101,21 +1101,33 @@ def getJointAnglesOfGroup(self, limb): # fix flag2groups for hironx(sim), which have gripper joints # flat2Groups: requres 315.2.0 # angles = self.flat2Groups(angles) + print(angles) offset = 0 + print(len(angles) , reduce(lambda x,y: x+len(y[1]), self.Groups, 0)) if len(angles) != reduce(lambda x,y: x+len(y[1]), self.Groups, 0): offset = 4 - angles = [] + group_angles = [] + #angles = [] index = 0 + if len(angles) != reduce(lambda x,y: x+len(y[1]), self.Groups, 0) + offset + offset: + index = 1 for group in self.Groups: + print(group) joint_num = len(group[1]) - angles.append(angles[index: index + joint_num]) + print("joint_num", group, joint_num, index, angles[index: index + joint_num]) + print(index) + group_angles.append(angles[index: index + joint_num]) + #angles.append(angles[index: index + joint_num]) + print("angles=",angles) index += joint_num if group[0] in ['larm', 'rarm']: index += offset ## FIX ME groups = self.Groups for i in range(len(groups)): if groups[i][0] == limb: - return angles[i] + #return angles[i] + print("return =================> ", i, group_angles[i]) + return group_angles[i] print self.configurator_name, 'could not find limb name ' + limb print self.configurator_name, ' in' + filter(lambda x: x[0], groups) diff --git a/hironx_ros_bridge/test/test_hironx_limb.py b/hironx_ros_bridge/test/test_hironx_limb.py index 268f8bfe..08143876 100755 --- a/hironx_ros_bridge/test/test_hironx_limb.py +++ b/hironx_ros_bridge/test/test_hironx_limb.py @@ -199,6 +199,16 @@ def test_rarm_setJointAnglesOfGroup_minus(self): self.robot.el_svc.setServoErrorLimit("all", 0.18) # default is 0.18 + def test_getJointAnglesOfGroup(self): + self.robot.setJointAnglesOfGroup("torso", [-1.234], 3, wait=False) + self.robot.setJointAnglesOfGroup("head", [5.6789, -10.1112], 3, wait=False) + self.robot.setJointAnglesOfGroup("rarm", [-1.6, 0, -100, 15.2, 9.4,-3.2], 3, wait=False) + self.robot.setJointAnglesOfGroup("larm", [ 1.6, 0, -100,-15.2, 9.4, 3.2], 3, wait=True) + numpy.testing.assert_array_almost_equal([-1.234], self.robot.getJointAnglesOfGroup("torso")) + numpy.testing.assert_array_almost_equal([5.6789, -10.1112], self.robot.getJointAnglesOfGroup("head")) + numpy.testing.assert_array_almost_equal([-1.6, 0, -100, 15.2, 9.4,-3.2], self.robot.getJointAnglesOfGroup("rarm")) + numpy.testing.assert_array_almost_equal([ 1.6, 0, -100,-15.2, 9.4, 3.2], self.robot.getJointAnglesOfGroup("larm")) + def test_movejoints_neck_waist(self): ''' Move neck and waist joints to the positional limit defined in