[WIP] Enable hrpsys collision detection.

tork-a · Mar 20, 2016 · ff1dd86 · ff1dd86
1 parent a0ff317
commit ff1dd86
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 1 deletion.
diff --git a/nextage_ros_bridge/src/nextage_ros_bridge/nextage_client.py b/nextage_ros_bridge/src/nextage_ros_bridge/nextage_client.py
@@ -322,7 +322,7 @@ def getRTCList(self):
             ['sh', "StateHolder"],
             ['fk', "ForwardKinematics"],
             ['el', "SoftErrorLimiter"],
-            # ['co', "CollisionDetector"],
+            ['co', "CollisionDetector"],
             # ['sc', "ServoController"],
             ['log', "DataLogger"]
             ]

diff --git a/nextage_ros_bridge/test/test_nxopen.py b/nextage_ros_bridge/test/test_nxopen.py
@@ -95,6 +95,20 @@ def test_set_targetpose_relative_dz(self):
         self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
         assert(self._set_relative(dz=0.0001))
 
+    def test_hrpsys_collisiondetection(self):
+        '''
+        Passing the pose that left arm collides into the right arm, so
+        this test succeeds when the command fails (, which is hopefully due
+        to CollisionDetector of hrpsys.
+        Image: https://drive.google.com/file/d/0Bw_hbVS3wTLyc1FHclFxbkcydTA/view?usp=sharing
+        '''
+        _dx_collision = -0.1
+        _dy_collision = -0.4
+        self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
+        result = self._robot.setTargetPoseRelative(
+                _ARMGROUP_TESTED, _LINK_TESTED, _dx_collision, _dy_collision, tm=3.0)
+        self.assertFalse(result)
+
 if __name__ == '__main__':
     import rostest
     rostest.rosrun(_PKG, 'test_nxopen', TestNextageopen)