Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

enable RTC collision detector on simulation by default #515

Open
wants to merge 13 commits into
base: indigo-devel
Choose a base branch
from
Open

enable RTC collision detector on simulation by default #515

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
b07bdd7
launch,hironx_ros_bridge: enable RTC/CollisionDetector on simulation …
k-okada Jul 3, 2017
c42f6c0
test/test_hironx.py add co for rtclist on test_hironx.py
k-okada Jul 3, 2017
31916c8
add test_hironx_collision.py
k-okada Jul 3, 2017
b77ef88
test/test_hironx_target.py: add commet to how to run individual test
k-okada Jul 3, 2017
e534810
test/test_hironx_target.py: print commet for all iteration
k-okada Jul 3, 2017
7a4bb14
test/test_hironx_target.py: add 0.2 [msec] offset for setTargetPoseRe…
k-okada Jul 3, 2017
4d441bd
skip tolerance test before 315.10.0
k-okada Jul 4, 2017
5aa56cb
test/test_hironx_collision.py : add comment to how to run individual …
k-okada Jul 4, 2017
3e5bc5e
conf/*.xml.in: remove longfloor
k-okada Jul 5, 2017
0a95f4a
conf/*.xml.in: chane home eye position
k-okada Jul 5, 2017
1293f35
relax test_hironx_limb.TestHiroLimb.test_rarm_setJointAngles_Clear test
k-okada Jul 5, 2017
bca6c76
test/test-hironx-ros-bridge.test: add retry=2
k-okada Jul 5, 2017
42de0f1
test/test-hironx-ros-bridge.test: joint_states_test/hzerror to 100, s…
k-okada Jul 5, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 1 addition & 39 deletions hironx_ros_bridge/conf/nosim.xml.in
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -121,54 +121,16 @@
<property name="showActualState" value="true"/>
<property name="showScale" value="true"/>
</view>
<item class="com.generalrobotix.ui.item.GrxRTSItem" name="longfloor" select="true">
<property name="longfloor(Robot)0.period" value="0.005"/>
<property name="HGcontroller0.period" value="0.005"/>
<property name="HGcontroller0.factory" value="HGcontroller"/>
<property name="connection" value="HGcontroller0.qOut:longfloor(Robot)0.qRef"/>
<property name="connection" value="HGcontroller0.dqOut:longfloor(Robot)0.dqRef"/>
<property name="connection" value="HGcontroller0.ddqOut:longfloor(Robot)0.ddqRef"/>
</item>
<item class="com.generalrobotix.ui.item.GrxModelItem" name="longfloor" url="@OPENHRP3@/share/OpenHRP-3.1/sample/model/longfloor.wrl">
<property name="rtcName" value="longfloor(Robot)0"/>
<property name="inport" value="qRef:JOINT_VALUE"/>
<property name="inport" value="dqRef:JOINT_VELOCITY"/>
<property name="inport" value="ddqRef:JOINT_ACCELERATION"/>
<property name="outport" value="q:JOINT_VALUE"/>
<property name="outport" value="tau:JOINT_TORQUE"/>
<property name="WAIST.NumOfAABB" value="1"/>
<property name="WAIST.translation" value="0 0 -0.1"/>
<property name="WAIST.rotation" value="1 0 0 0"/>
</item>
<view class="com.generalrobotix.ui.view.GrxRobotHardwareClientView" name="RobotHardware RTC Client">
<property name="robotHost" value="localhost"/>
<property name="StateHolderRTC" value="StateHolder0"/>
<property name="interval" value="100"/>
<property name="RobotHardwareServiceRTC" value="RobotHardware0"/>
<property name="robotPort" value="2809"/>
<property name="ROBOT" value="longfloor"/>
</view>
<view class="com.generalrobotix.ui.view.Grx3DView" name="3DView">
<property name="view.mode" value="Room"/>
<property name="showCoM" value="false"/>
<property name="showCoMonFloor" value="false"/>
<property name="showDistance" value="false"/>
<property name="showIntersection" value="false"/>
<property name="eyeHomePosition" value="-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.8 0 0 0 1 "/>
<property name="eyeHomePosition" value="-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.3 0 0 0 1 "/>
<property name="showCollision" value="true"/>
<property name="showActualState" value="true"/>
<property name="showScale" value="true"/>
</view>
<item class="com.generalrobotix.ui.item.GrxCollisionPairItem" name="CP#longfloor_#@ROBOT_NAME@_">
<property name="springConstant" value="0 0 0 0 0 0"/>
<property name="slidingFriction" value="0.5"/>
<property name="jointName2" value=""/>
<property name="jointName1" value=""/>
<property name="damperConstant" value="0 0 0 0 0 0"/>
<property name="objectName2" value="longfloor"/>
<property name="objectName1" value="@ROBOT_NAME@"/>
<property name="springDamperModel" value="false"/>
<property name="staticFriction" value="0.5"/>
</item>
</mode>
</grxui>
40 changes: 1 addition & 39 deletions hironx_ros_bridge/conf/xml.in
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -121,54 +121,16 @@
<property name="showActualState" value="true"/>
<property name="showScale" value="true"/>
</view>
<item class="com.generalrobotix.ui.item.GrxRTSItem" name="longfloor" select="true">
<property name="longfloor(Robot)0.period" value="0.005"/>
<property name="HGcontroller0.period" value="0.005"/>
<property name="HGcontroller0.factory" value="HGcontroller"/>
<property name="connection" value="HGcontroller0.qOut:longfloor(Robot)0.qRef"/>
<property name="connection" value="HGcontroller0.dqOut:longfloor(Robot)0.dqRef"/>
<property name="connection" value="HGcontroller0.ddqOut:longfloor(Robot)0.ddqRef"/>
</item>
<item class="com.generalrobotix.ui.item.GrxModelItem" name="longfloor" url="@OPENHRP3@/share/OpenHRP-3.1/sample/model/longfloor.wrl">
<property name="rtcName" value="longfloor(Robot)0"/>
<property name="inport" value="qRef:JOINT_VALUE"/>
<property name="inport" value="dqRef:JOINT_VELOCITY"/>
<property name="inport" value="ddqRef:JOINT_ACCELERATION"/>
<property name="outport" value="q:JOINT_VALUE"/>
<property name="outport" value="tau:JOINT_TORQUE"/>
<property name="WAIST.NumOfAABB" value="1"/>
<property name="WAIST.translation" value="0 0 -0.1"/>
<property name="WAIST.rotation" value="1 0 0 0"/>
</item>
<view class="com.generalrobotix.ui.view.GrxRobotHardwareClientView" name="RobotHardware RTC Client">
<property name="robotHost" value="localhost"/>
<property name="StateHolderRTC" value="StateHolder0"/>
<property name="interval" value="100"/>
<property name="RobotHardwareServiceRTC" value="RobotHardware0"/>
<property name="robotPort" value="2809"/>
<property name="ROBOT" value="longfloor"/>
</view>
<view class="com.generalrobotix.ui.view.Grx3DView" name="3DView">
<property name="view.mode" value="Room"/>
<property name="showCoM" value="false"/>
<property name="showCoMonFloor" value="false"/>
<property name="showDistance" value="false"/>
<property name="showIntersection" value="false"/>
<property name="eyeHomePosition" value="-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.8 0 0 0 1 "/>
<property name="eyeHomePosition" value="-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.3 0 0 0 1 "/>
<property name="showCollision" value="true"/>
<property name="showActualState" value="true"/>
<property name="showScale" value="true"/>
</view>
<item class="com.generalrobotix.ui.item.GrxCollisionPairItem" name="CP#longfloor_#@ROBOT_NAME@_">
<property name="springConstant" value="0 0 0 0 0 0"/>
<property name="slidingFriction" value="0.5"/>
<property name="jointName2" value=""/>
<property name="jointName1" value=""/>
<property name="damperConstant" value="0 0 0 0 0 0"/>
<property name="objectName2" value="longfloor"/>
<property name="objectName1" value="@ROBOT_NAME@"/>
<property name="springDamperModel" value="false"/>
<property name="staticFriction" value="0.5"/>
</item>
</mode>
</grxui>
2 changes: 1 addition & 1 deletion hironx_ros_bridge/launch/hironx_ros_bridge.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@

<!-- Collision detection here runs external to the robot (on Ubuntu) and
visualizes the distance between each links. -->
<include file="$(find hrpsys_ros_bridge)/launch/collision_detector.launch">
<include file="$(find hrpsys_ros_bridge)/launch/collision_detector.launch" unless="$(arg USE_COLLISIONCHECK)" >
<arg name="MODEL_FILE" value="$(arg MODEL_FILE)" />
<arg name="CONF_FILE" value="$(arg CONF_FILE_COLLISIONDETECT)" />
<arg name="corbaport" default="$(arg corbaport)" />
Expand Down
3 changes: 3 additions & 0 deletions hironx_ros_bridge/launch/hironx_ros_bridge_simulation.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
<arg name="CONF_FILE" default="$(find hironx_ros_bridge)/conf/kawada-hironx.conf" />
<arg name="CONF_FILE_COLLISIONDETECT" default="$(find hironx_ros_bridge)/conf/kawada-hironx.conf" />
<arg name="DEBUG_HRPSYS" default="false" />
<arg name="USE_COLLISIONCHECK" default="true" /> <!-- enable collision rtc on simulation -->

<!-- Set model file to parameter server -->
<param name="hironx/collada_model_filepath" value="$(arg MODEL_FILE)" />
Expand All @@ -15,13 +16,15 @@
<arg name="corbaport" default="$(arg corbaport)" />
<arg name="CONF_FILE" value="$(arg CONF_FILE)" />
<arg name="DEBUG_HRPSYS" value="$(arg DEBUG_HRPSYS)" />
<arg name="USE_COLLISIONCHECK" default="$(arg USE_COLLISIONCHECK)" />
</include>
<include file="$(find hironx_ros_bridge)/launch/hironx_ros_bridge.launch" >
<arg name="nameserver" value="localhost" />
<arg name="MODEL_FILE" value="$(arg MODEL_FILE)" />
<arg name="SIMULATOR_NAME" value="HiroNX(Robot)0" />
<arg name="corbaport" default="$(arg corbaport)" />
<arg name="CONF_FILE_COLLISIONDETECT" value="$(arg CONF_FILE_COLLISIONDETECT)" />
<arg name="USE_COLLISIONCHECK" default="$(arg USE_COLLISIONCHECK)" />
<arg name="open_rqt_gui" default="false" />
</include>

Expand Down
4 changes: 4 additions & 0 deletions hironx_ros_bridge/launch/hironx_startup.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,9 @@
<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="USE_COLLISIONCHECK" default="true" /> <!-- enable collision rtc on simulation -->
<arg name="HRPSYS_PY_ARGS" default="--use-collision" if="$(arg USE_COLLISIONCHECK)" />
<arg name="HRPSYS_PY_ARGS" default="" unless="$(arg USE_COLLISIONCHECK)" />
<arg name="corbaport" default="15005" />
<arg name="DEBUG_HRPSYS" default="false" />

Expand All @@ -23,6 +26,7 @@
<arg name="LAUNCH_HRPSYSPY" value="$(arg LAUNCH_HRPSYSPY)" />
<arg name="HRPSYS_PY_PKG" value="$(arg HRPSYS_PY_PKG)"/>
<arg name="HRPSYS_PY_NAME" value="$(arg HRPSYS_PY_NAME)"/>
<arg name="HRPSYS_PY_ARGS" value="$(arg HRPSYS_PY_ARGS)"/>
<arg name="REALTIME" value="$(arg REALTIME)" />
<arg name="corbaport" value="$(arg corbaport)" />
<arg name="DEBUG_HRPSYS" value="$(arg DEBUG_HRPSYS)" />
Expand Down
3 changes: 3 additions & 0 deletions hironx_ros_bridge/scripts/hironx.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@
parser.add_argument('--port', help='corba name server port number')
parser.add_argument('--modelfile', help='robot model file nmae')
parser.add_argument('--robot', help='robot modlule name (RobotHardware0 for real robot, Robot()')
parser.add_argument('--use-collision', help='install collision detector', action='store_true', default=False)
args, unknown = parser.parse_known_args()
unknown = [u for u in unknown if u[:2] != '__'] # filter out ros arguments

Expand All @@ -79,6 +80,8 @@
args.robot = unknown[0]
args.modelfile = unknown[1]
robot = hiro = hironx_client.HIRONX()
if args.use_collision:
robot.rtclist.append(['co', "CollisionDetector"])
robot.init(robotname=args.robot, url=args.modelfile)

# ROS Client
Expand Down
21 changes: 11 additions & 10 deletions hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -319,6 +319,16 @@ class via the link above; nicely formatted api doc web page
"the function call was successful, since not " +
"all methods internally called return status")

rtclist = [
['seq', "SequencePlayer"],
['sh', "StateHolder"],
['fk', "ForwardKinematics"],
['ic', "ImpedanceController"],
['el', "SoftErrorLimiter"],
# ['co', "CollisionDetector"],
['sc', "ServoController"],
['log', "DataLogger"],
]
def init(self, robotname="HiroNX(Robot)0", url=""):
'''
Calls init from its superclass, which tries to connect RTCManager,
Expand Down Expand Up @@ -442,16 +452,7 @@ 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"],
['sc', "ServoController"],
['log', "DataLogger"],
]
rtclist = self.rtclist
if hasattr(self, 'rmfo'):
self.ms.load("RemoveForceSensorLinkOffset")
self.ms.load("AbsoluteForceSensor")
Expand Down
5 changes: 3 additions & 2 deletions hironx_ros_bridge/test/test-hironx-ros-bridge.test
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,13 +9,14 @@

<param name="joint_states_test/topic" value="/joint_states" />
<param name="joint_states_test/hz" value="200" />
<param name="joint_states_test/hzerror" value="10" />
<param name="joint_states_test/hzerror" value="100" />
<param name="joint_states_test/test_duration" value="5.0" />
<param name="joint_states_test/wait_time" value="60.0" /> <!-- as opposed to its default: 20.0 -->

<test test-name="joint_states_test" pkg="rostest" type="hztest" name="joint_states_test" time-limit="100"/>
<test test-name="joint_states_test" pkg="rostest" type="hztest" name="joint_states_test" time-limit="100" retry="2"/>

<test test-name="test_no_moveit" pkg="hironx_ros_bridge" type="test_no_moveit.py" name="test_no_moveit"
retry="2"
args="HiroNX(Robot)0
$(find hironx_ros_bridge)/models/kawada-hironx.dae
-ORBInitRef NameService=corbaloc:iiop:localhost:$(arg corbaport)/NameService"
Expand Down
4 changes: 3 additions & 1 deletion hironx_ros_bridge/test/test-hironx.test
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@

<test type="test_hironx_fullbody.py" pkg="hironx_ros_bridge" test-name="test_hironx_fullbody" time-limit="1000" retry="2"
args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
<test type="test_hironx_limb.py" pkg="hironx_ros_bridge" test-name="test_hironx_limb" time-limit="200" retry="2"
<test type="test_hironx_limb.py" pkg="hironx_ros_bridge" test-name="test_hironx_limb" time-limit="200" retry="4"
args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
<test type="test_hironx_target.py" pkg="hironx_ros_bridge" test-name="test_hironx_target" time-limit="200" retry="2"
args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
Expand All @@ -25,4 +25,6 @@
args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
<test type="test_hironx_ik_noinit.py" pkg="hironx_ros_bridge" test-name="test_hironx_ik_noinit" time-limit="3000"
args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
<test type="test_hironx_collision.py" pkg="hironx_ros_bridge" test-name="test_hironx_collision" time-limit="600"
args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
</launch>
8 changes: 5 additions & 3 deletions hironx_ros_bridge/test/test_hironx.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,8 @@ def setUpClass(cls):
#robotname = "RobotHardware0"

cls.robot = hironx.HIRONX()
if not ['co', "CollisionDetector"] in cls.robot.rtclist:
cls.robot.rtclist.append(['co', "CollisionDetector"])
#cls.robot.init(robotname=robotname, url=modelfile)
cls.robot.init()

Expand Down Expand Up @@ -119,9 +121,9 @@ def check_log_data(self, data, idx, tm_data, min_data, max_data, acc_thre=0.06,
if isinstance(max_data, (int, float)):
max_data = [max_data, 5]

print "time (= ", _tm_data, ") == ", tm_data, " -> ", abs(_tm_data - tm_data) < tm_data*_tm_thre
print " min (= ", _min_data, ") == ", min_data, " -> ", abs(_min_data - min_data[0]) < min_data[1]
print " max (= ", _max_data, ") == ", max_data, " -> ", abs(_max_data - max_data[0]) < max_data[1]
print "time (= ", _tm_data, ") == ", tm_data, " -> ", abs(_tm_data - tm_data) < tm_data*_tm_thre, tm_data*_tm_thre - abs(_tm_data - tm_data)
print " min (= ", _min_data, ") == ", min_data, " -> ", abs(_min_data - min_data[0]) < min_data[1], min_data[1] - abs(_min_data - min_data[0])
print " max (= ", _max_data, ") == ", max_data, " -> ", abs(_max_data - max_data[0]) < max_data[1], max_data[1] - abs(_max_data - max_data[0])
self.assertTrue(abs(_tm_data - tm_data) < tm_data*_tm_thre)
self.assertTrue(abs(_min_data - min_data[0]) < min_data[1])
self.assertTrue(abs(_max_data - max_data[0]) < max_data[1])
Expand Down
Loading