add test for getJointAnglesOfGroup

start-jsk · Jul 24, 2018 · cc2b65a · cc2b65a
1 parent 8d5c2ce
commit cc2b65a
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 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
@@ -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
@@ -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