Add collision detection input param and test file

hironx_ros_bridge/conf/conf.in

@@ -1,5 +1,6 @@
 model: file://@PROJECT_SOURCE_DIR@/models/@ROBOT_NAME@.dae
 dt: 0.005
-collision_pair: RARM_JOINT2:LARM_JOINT2 RARM_JOINT3:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5
+collision_pair: RARM_JOINT1:RARM_JOINT3 RARM_JOINT1:RARM_JOINT4 RARM_JOINT3:RARM_JOINT5 RARM_JOINT3:RHAND_JOINT0 RARM_JOINT4:RHAND_JOINT0 RARM_JOINT4:RHAND_JOINT1 RARM_JOINT4:RHAND_JOINT2 RARM_JOINT4:RHAND_JOINT3 RARM_JOINT5:RHAND_JOINT3 RHAND_JOINT0:RHAND_JOINT2 LARM_JOINT1:LARM_JOINT3 LARM_JOINT1:LARM_JOINT4 LARM_JOINT3:LARM_JOINT5 LARM_JOINT3:LHAND_JOINT2 LARM_JOINT4:LHAND_JOINT0 LARM_JOINT4:LHAND_JOINT1 LARM_JOINT4:LHAND_JOINT2 LARM_JOINT4:LHAND_JOINT3 LARM_JOINT5:LHAND_JOINT1 LHAND_JOINT0:LHAND_JOINT2 RHAND_JOINT3:LHAND_JOINT1 RHAND_JOINT3:LHAND_JOINT3 RHAND_JOINT1:LHAND_JOINT3 RHAND_JOINT1:LHAND_JOINT1 RHAND_JOINT0:LHAND_JOINT3 RHAND_JOINT2:LHAND_JOINT3 RHAND_JOINT2:LHAND_JOINT1 RHAND_JOINT3:LHAND_JOINT0 RHAND_JOINT3:LHAND_JOINT2 RHAND_JOINT1:LHAND_JOINT2 RHAND_JOINT1:LHAND_JOINT0 RHAND_JOINT0:LHAND_JOINT1 RHAND_JOINT1:LARM_JOINT5 RARM_JOINT5:LHAND_JOINT1 RARM_JOINT5:LHAND_JOINT3 RHAND_JOINT3:LARM_JOINT5 RHAND_JOINT2:LHAND_JOINT2 RHAND_JOINT0:LHAND_JOINT2 RHAND_JOINT0:LHAND_JOINT0 RHAND_JOINT2:LHAND_JOINT0 RARM_JOINT5:LHAND_JOINT2 RHAND_JOINT1:LARM_JOINT4 RHAND_JOINT0:LARM_JOINT5 RHAND_JOINT2:LARM_JOINT5 RARM_JOINT4:LHAND_JOINT1 RARM_JOINT4:LHAND_JOINT3 RHAND_JOINT3:LARM_JOINT4 RARM_JOINT5:LHAND_JOINT0 RHAND_JOINT2:LARM_JOINT4 RHAND_JOINT0:LARM_JOINT4 RHAND_JOINT3:LARM_JOINT3 RARM_JOINT3:LHAND_JOINT1 RARM_JOINT3:LHAND_JOINT3 RARM_JOINT5:LARM_JOINT5 RARM_JOINT4:LHAND_JOINT2 RARM_JOINT4:LHAND_JOINT0 RHAND_JOINT1:LARM_JOINT3 RARM_JOINT3:LHAND_JOINT2 RHAND_JOINT3:LARM_JOINT2 RHAND_JOINT0:LARM_JOINT3 RARM_JOINT2:LHAND_JOINT3 RARM_JOINT2:LHAND_JOINT1 RARM_JOINT5:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT3:LHAND_JOINT0 RHAND_JOINT1:LARM_JOINT2 RHAND_JOINT2:LARM_JOINT3 HEAD_JOINT1:RHAND_JOINT3 HEAD_JOINT1:RHAND_JOINT1 RARM_JOINT4:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RHAND_JOINT1:LARM_JOINT1 HEAD_JOINT1:LHAND_JOINT1 HEAD_JOINT1:LHAND_JOINT3 RARM_JOINT5:LARM_JOINT3 RHAND_JOINT3:LARM_JOINT1 RARM_JOINT1:LHAND_JOINT1 RHAND_JOINT0:LARM_JOINT2 RARM_JOINT1:LHAND_JOINT3 RARM_JOINT2:LHAND_JOINT2 RARM_JOINT2:LHAND_JOINT0 RHAND_JOINT2:LARM_JOINT2 RHAND_JOINT0:LARM_JOINT1 RARM_JOINT1:LHAND_JOINT0 RARM_JOINT2:LARM_JOINT5 RHAND_JOINT2:LARM_JOINT1 RARM_JOINT5:LARM_JOINT2 RARM_JOINT3:LARM_JOINT4 RARM_JOINT4:LARM_JOINT3 RARM_JOINT1:LHAND_JOINT2 HEAD_JOINT1:RHAND_JOINT2 HEAD_JOINT1:RHAND_JOINT0 HEAD_JOINT1:LHAND_JOINT0 HEAD_JOINT1:LHAND_JOINT2 HEAD_JOINT1:RARM_JOINT5 WAIST:LHAND_JOINT1 RARM_JOINT2:LARM_JOINT4 RARM_JOINT1:LARM_JOINT5 RARM_JOINT3:LARM_JOINT3 WAIST:RHAND_JOINT3 WAIST:LHAND_JOINT3 WAIST:RHAND_JOINT1 HEAD_JOINT1:LARM_JOINT5 RARM_JOINT5:LARM_JOINT1 RARM_JOINT4:LARM_JOINT2 CHEST_JOINT0:RHAND_JOINT1 CHEST_JOINT0:RHAND_JOINT3 WAIST:LHAND_JOINT0 RARM_JOINT4:LARM_JOINT1 CHEST_JOINT0:LHAND_JOINT1 CHEST_JOINT0:LHAND_JOINT3 WAIST:LHAND_JOINT2 RARM_JOINT3:LARM_JOINT2 RARM_JOINT1:LARM_JOINT4 WAIST:RHAND_JOINT0 HEAD_JOINT1:LARM_JOINT4 WAIST:RHAND_JOINT2 HEAD_JOINT1:RARM_JOINT4 RARM_JOINT2:LARM_JOINT3 CHEST_JOINT0:RHAND_JOINT2 LARM_JOINT0:LHAND_JOINT3 CHEST_JOINT0:LHAND_JOINT0 RARM_JOINT0:RHAND_JOINT3 RARM_JOINT0:RHAND_JOINT1 CHEST_JOINT0:RHAND_JOINT0 WAIST:RARM_JOINT5 CHEST_JOINT0:LHAND_JOINT2 LARM_JOINT0:LHAND_JOINT1 HEAD_JOINT1:LARM_JOINT3 RARM_JOINT1:LARM_JOINT3 RARM_JOINT3:LARM_JOINT1 HEAD_JOINT1:RARM_JOINT3 RARM_JOINT2:LARM_JOINT2 WAIST:LARM_JOINT5 LARM_JOINT1:LHAND_JOINT1 LARM_JOINT1:LHAND_JOINT3 CHEST_JOINT0:RARM_JOINT5 WAIST:RARM_JOINT4 WAIST:LARM_JOINT4 RARM_JOINT0:RHAND_JOINT2 RARM_JOINT1:RHAND_JOINT1 RARM_JOINT1:RHAND_JOINT3 HEAD_JOINT1:LARM_JOINT2 RARM_JOINT0:RHAND_JOINT0 RARM_JOINT1:LARM_JOINT2 HEAD_JOINT1:RARM_JOINT2 RARM_JOINT2:LARM_JOINT1 LARM_JOINT0:LHAND_JOINT0 LARM_JOINT0:LHAND_JOINT2 CHEST_JOINT0:LARM_JOINT5 LARM_JOINT2:LHAND_JOINT3 LARM_JOINT1:LHAND_JOINT0 LARM_JOINT2:LHAND_JOINT1 LARM_JOINT0:LARM_JOINT5 LARM_JOINT1:LHAND_JOINT2 RARM_JOINT2:RHAND_JOINT1 HEAD_JOINT1:RARM_JOINT1 HEAD_JOINT1:LARM_JOINT1 HEAD_JOINT0:LARM_JOINT2 RARM_JOINT0:RARM_JOINT5 HEAD_JOINT0:RARM_JOINT2 CHEST_JOINT0:LARM_JOINT4 RARM_JOINT1:RHAND_JOINT0 RARM_JOINT1:RHAND_JOINT2 RARM_JOINT1:LARM_JOINT1 CHEST_JOINT0:RARM_JOINT4 RARM_JOINT2:RHAND_JOINT3 WAIST:LARM_JOINT3 WAIST:RARM_JOINT3 WAIST:RARM_JOINT2 LARM_JOINT0:LARM_JOINT4 LARM_JOINT3:LHAND_JOINT3 LARM_JOINT3:LHAND_JOINT1 LARM_JOINT2:LHAND_JOINT2 WAIST:LARM_JOINT2 LARM_JOINT2:LHAND_JOINT0 LARM_JOINT1:LARM_JOINT5 CHEST_JOINT0:LARM_JOINT3 HEAD_JOINT1:RARM_JOINT0 CHEST_JOINT0:RARM_JOINT3 RARM_JOINT3:RHAND_JOINT3 RARM_JOINT3:RHAND_JOINT1 RARM_JOINT2:RHAND_JOINT2 RARM_JOINT2:RHAND_JOINT0 HEAD_JOINT1:LARM_JOINT0 RARM_JOINT1:RARM_JOINT5 RARM_JOINT0:RARM_JOINT4 LARM_JOINT1:LARM_JOINT4 LARM_JOINT4:LHAND_JOINT3 LARM_JOINT4:LHAND_JOINT1 RARM_JOINT4:RHAND_JOINT3 LARM_JOINT3:LHAND_JOINT2 RARM_JOINT4:RHAND_JOINT1 RARM_JOINT3:RHAND_JOINT0 RARM_JOINT1:RARM_JOINT4 LARM_JOINT3:LARM_JOINT5 RARM_JOINT4:RHAND_JOINT2 RARM_JOINT4:RHAND_JOINT0 LARM_JOINT4:LHAND_JOINT0 LARM_JOINT4:LHAND_JOINT2 RARM_JOINT3:RARM_JOINT5 LARM_JOINT5:LHAND_JOINT1 RARM_JOINT1:RARM_JOINT3 RARM_JOINT5:RHAND_JOINT3 LARM_JOINT1:LARM_JOINT3
 end_effectors: rarm,RARM_JOINT5,CHEST_JOINT0,0.0,0.0,0.0,-0.57735027,-0.57735027,-0.57735027,-2.0943951023931953,larm,LARM_JOINT5,CHEST_JOINT0,0.0,0.0,0.0,-0.57735027,-0.57735027,-0.57735027,-2.0943951023931953,
+collision_library: fcl
 

hironx_ros_bridge/launch/hironx_startup.launch

@@ -8,8 +8,10 @@
   <arg name="SIMULATOR_NAME" default="HiroNX(Robot)0" />
   <arg name="HRPSYS_PY_PKG" default="hironx_ros_bridge"/>
   <arg name="HRPSYS_PY_NAME" default="hironx.py"/>
+  <arg name="HRPSYS_PY_ARGS" default="" />
   <arg name="corbaport" default="15005" />
   <arg name="DEBUG_HRPSYS" default="false" />
+  <arg name="COLLISION_DETECTION" default="true" />
 
   <!-- begin use sim time -->
   <param name="use_sim_time" value="true" />
@@ -26,5 +28,6 @@
     <arg name="REALTIME" value="$(arg REALTIME)" />
     <arg name="corbaport" value="$(arg corbaport)" />
     <arg name="DEBUG_HRPSYS" value="$(arg DEBUG_HRPSYS)" />
+    <arg name="COLLISION_DETECTION" value="$(arg COLLISION_DETECTION)" />
   </include>
 </launch>

hironx_ros_bridge/scripts/hironx.py

@@ -78,8 +78,9 @@
     if len(unknown) >= 2:
         args.robot = unknown[0]
         args.modelfile = unknown[1]
+        args.collision = (unknown[4].lower() == 'true')
     robot = hiro = hironx_client.HIRONX()
-    robot.init(robotname=args.robot, url=args.modelfile)
+    robot.init(robotname=args.robot, url=args.modelfile, collision=args.collision)
 
     # ROS Client
     try:

hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py

@@ -319,7 +319,9 @@ class via the link above; nicely formatted api doc web page
                                "the function call was successful, since not " +
                                "all methods internally called return status")
 
-    def init(self, robotname="HiroNX(Robot)0", url=""):
+    _collision = False
+
+    def init(self, robotname="HiroNX(Robot)0", url="", collision=False):
         '''
         Calls init from its superclass, which tries to connect RTCManager,
         looks for ModelLoader, and starts necessary RTC components. Also runs
@@ -330,6 +332,9 @@ def init(self, robotname="HiroNX(Robot)0", url=""):
         @type url: str
         '''
         # reload for hrpsys 315.1.8
+
+        self._collision = collision
+
         print(self.configurator_name + "waiting ModelLoader")
         HrpsysConfigurator.waitForModelLoader(self)
         print(self.configurator_name + "start hrpsys")
@@ -384,6 +389,7 @@ def init(self, robotname="HiroNX(Robot)0", url=""):
         self.setSelfGroups()
         self.hrpsys_version = self.fk.ref.get_component_profile().version
 
+
     def goOffPose(self, tm=7):
         '''
         Move arms to the predefined (as member variable) pose where robot can
@@ -442,16 +448,18 @@ def getRTCList(self):
         @rerutrn List of available components. Each element consists of a list
                  of abbreviated and full names of the component.
         '''
+
         rtclist = [
             ['seq', "SequencePlayer"],
             ['sh', "StateHolder"],
             ['fk', "ForwardKinematics"],
             ['ic', "ImpedanceController"],
             ['el', "SoftErrorLimiter"],
-            # ['co', "CollisionDetector"],
+            ['co', "CollisionDetector"],
             ['sc', "ServoController"],
             ['log', "DataLogger"],
             ]
+
         if hasattr(self, 'rmfo'):
             self.ms.load("RemoveForceSensorLinkOffset")
             self.ms.load("AbsoluteForceSensor")
@@ -461,6 +469,13 @@ def getRTCList(self):
                 rtclist.append(['rmfo', "AbsoluteForceSensor"])
             else:
                 print "Component rmfo is not loadable."
+
+        if self._collision:
+            print "\nCollision Detection on \n"
+        else:
+            print "\nCollision Detection off\n"
+            rtclist.remove(['co', "CollisionDetector"])
+
         return rtclist
 
     # hand interface

hironx_ros_bridge/test/test_hironx_collision.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import unittest
+import numpy
+
+from hironx_ros_bridge import hironx_client as hironx
+from hrpsys import rtm
+
+_ARMGROUP_TESTED = 'larm'
+_LINK_TESTED = 'LARM_JOINT5'
+_GOINITIAL_TIME_MIDSPEED = 3  # second
+_NUM_CARTESIAN_ITERATION = 300
+_DIFF_THRESHOLD = 0.001
+_PKG = 'nextage_ros_bridge'
+
+
+class TestNextageopen(unittest.TestCase):
+    '''
+    Test NextageClient with rostest.
+    '''
+
+    @classmethod
+    def setUpClass(self):
+
+        #modelfile = rospy.get_param("hironx/collada_model_filepath")
+        #rtm.nshost = 'hiro024'
+        #robotname = "RobotHardware0"
+
+        self._robot = hironx.HIRONX()
+        #self._robot.init(robotname=robotname, url=modelfile)
+        self._robot.init()
+
+        self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
+
+    def _set_collide(self, dx = 0, dy = 0, dz = 0):
+        self._robot.seq_svc.setMaxIKError(0.00001, 0.01)
+        init_pose = self._robot.getCurrentPosition(_LINK_TESTED)
+        self._robot.setTargetPoseRelative(_ARMGROUP_TESTED, _LINK_TESTED, dx, dy, dz, tm = 1)
+        curr_pose = self._robot.getCurrentPosition(_LINK_TESTED)
+
+        if (abs(init_pose[0] - curr_pose[0] + dx) > _DIFF_THRESHOLD) or (abs(init_pose[1] - curr_pose[1] + dy) > _DIFF_THRESHOLD) or (abs(init_pose[2] - curr_pose[2] + dz) > _DIFF_THRESHOLD):
+            print('Collision Detector enabled\n')
+            return True
+        else:
+            print('Collision Detector NOT enabled\n')
+            return False
+
+    # def test_collision_dx(self):
+    #     assert(self._set_collide(dx = -0.3))
+    #     self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
+
+    def test_collision_dy(self):
+        assert(self._set_collide(dy = -0.4))
+        self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
+
+    # def test_collision_dz(self):
+    #     assert(self._set_collide(dz = -0.3))
+    #     self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
+
+# unittest.main()
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(_PKG, 'test_collide', TestNextageopen)