From ff1dd863cc176e03473d71e854cd13858f6770cf Mon Sep 17 00:00:00 2001 From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp> Date: Sat, 19 Mar 2016 18:22:06 -0700 Subject: [PATCH] [WIP] Enable hrpsys collision detection. --- .../src/nextage_ros_bridge/nextage_client.py | 2 +- nextage_ros_bridge/test/test_nxopen.py | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 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 index 2d8e9338..be4fb10b 100755 --- 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 index 94deb95e..9b19a9f3 100755 --- 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)