From 49696ab37726928e0a0efd43407e4378013d01a9 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 24 Jul 2018 21:36:42 +0900 Subject: [PATCH 1/3] add test for getJointAnglesOfGroup --- hironx_ros_bridge/test/test_hironx_limb.py | 10 ++++++++++ 1 file changed, 10 insertions(+) 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 From c18fc4d8dfa40f6aaa810eb20a0b2d2de5789eda Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 24 Jul 2018 21:43:08 +0900 Subject: [PATCH 2/3] fix error message --- hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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..323a1568 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +++ b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py @@ -1117,7 +1117,7 @@ def getJointAnglesOfGroup(self, limb): if groups[i][0] == limb: return angles[i] print self.configurator_name, 'could not find limb name ' + limb - print self.configurator_name, ' in' + filter(lambda x: x[0], groups) + print self.configurator_name, ' in ' + str(map(lambda x: x[0], groups)) def clearOfGroup(self, limb): '''!@brief From c8555e38ab3bf77f9fad677618ad42afe0a2a228 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 24 Jul 2018 21:44:06 +0900 Subject: [PATCH 3/3] fix getJointAnglesOfGroup, not sure why #505 is not tested --- hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py | 8 +++++--- 1 file changed, 5 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 323a1568..f5556912 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +++ b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py @@ -1104,18 +1104,20 @@ def getJointAnglesOfGroup(self, limb): offset = 0 if len(angles) != reduce(lambda x,y: x+len(y[1]), self.Groups, 0): offset = 4 - angles = [] + group_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: joint_num = len(group[1]) - angles.append(angles[index: index + joint_num]) + group_angles.append(angles[index: index + joint_num]) 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 group_angles[i] print self.configurator_name, 'could not find limb name ' + limb print self.configurator_name, ' in ' + str(map(lambda x: x[0], groups))