add test for getJointAnglesOfGroup

start-jsk · Jul 24, 2018 · 49696ab · 49696ab
1 parent 8d5c2ce
commit 49696ab
Showing 1 changed file with 10 additions and 0 deletions.
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