diff --git a/hironx_ros_bridge/conf/nosim.xml.in b/hironx_ros_bridge/conf/nosim.xml.in
index a90effaa..e879bc6e 100644
--- a/hironx_ros_bridge/conf/nosim.xml.in
+++ b/hironx_ros_bridge/conf/nosim.xml.in
@@ -121,54 +121,16 @@
- -
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
- -
-
-
-
-
-
-
-
-
-
-
diff --git a/hironx_ros_bridge/conf/xml.in b/hironx_ros_bridge/conf/xml.in
index 40989a4b..3f9077b1 100644
--- a/hironx_ros_bridge/conf/xml.in
+++ b/hironx_ros_bridge/conf/xml.in
@@ -121,54 +121,16 @@
- -
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
- -
-
-
-
-
-
-
-
-
-
-
diff --git a/hironx_ros_bridge/launch/hironx_ros_bridge.launch b/hironx_ros_bridge/launch/hironx_ros_bridge.launch
index 2f3ea32a..4ca61635 100644
--- a/hironx_ros_bridge/launch/hironx_ros_bridge.launch
+++ b/hironx_ros_bridge/launch/hironx_ros_bridge.launch
@@ -47,7 +47,7 @@
-
+
diff --git a/hironx_ros_bridge/launch/hironx_ros_bridge_simulation.launch b/hironx_ros_bridge/launch/hironx_ros_bridge_simulation.launch
index 821287c8..506e10b0 100644
--- a/hironx_ros_bridge/launch/hironx_ros_bridge_simulation.launch
+++ b/hironx_ros_bridge/launch/hironx_ros_bridge_simulation.launch
@@ -5,6 +5,7 @@
+
@@ -15,6 +16,7 @@
+
@@ -22,6 +24,7 @@
+
diff --git a/hironx_ros_bridge/launch/hironx_startup.launch b/hironx_ros_bridge/launch/hironx_startup.launch
index 61083450..03ef88d6 100644
--- a/hironx_ros_bridge/launch/hironx_startup.launch
+++ b/hironx_ros_bridge/launch/hironx_startup.launch
@@ -8,6 +8,9 @@
+
+
+
@@ -23,6 +26,7 @@
+
diff --git a/hironx_ros_bridge/scripts/hironx.py b/hironx_ros_bridge/scripts/hironx.py
index d2602670..7763ddb0 100755
--- a/hironx_ros_bridge/scripts/hironx.py
+++ b/hironx_ros_bridge/scripts/hironx.py
@@ -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
@@ -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
diff --git a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py
index fc62babf..e7332fa1 100644
--- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py
+++ b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py
@@ -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,
@@ -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")
diff --git a/hironx_ros_bridge/test/test-hironx-ros-bridge.test b/hironx_ros_bridge/test/test-hironx-ros-bridge.test
index 06e4c848..479d1ecf 100644
--- a/hironx_ros_bridge/test/test-hironx-ros-bridge.test
+++ b/hironx_ros_bridge/test/test-hironx-ros-bridge.test
@@ -9,13 +9,14 @@
-
+
-
+
-
@@ -25,4 +25,6 @@
args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
+
diff --git a/hironx_ros_bridge/test/test_hironx.py b/hironx_ros_bridge/test/test_hironx.py
index e1382f42..34c612ee 100755
--- a/hironx_ros_bridge/test/test_hironx.py
+++ b/hironx_ros_bridge/test/test_hironx.py
@@ -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()
@@ -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])
diff --git a/hironx_ros_bridge/test/test_hironx_collision.py b/hironx_ros_bridge/test/test_hironx_collision.py
new file mode 100755
index 00000000..b1aa8239
--- /dev/null
+++ b/hironx_ros_bridge/test/test_hironx_collision.py
@@ -0,0 +1,177 @@
+#!/usr/bin/env python
+
+
+from test_hironx import TestHiro
+import sys
+import math
+
+from distutils.version import StrictVersion
+
+PKG = 'hironx_ros_bridge'
+
+def vector_equal_eps (vec1, vec2, eps=1e-5):
+ if len(vec1) == len(vec2):
+ for e1, e2 in zip(vec1, vec2):
+ if abs(e1 - e2) > eps:
+ return False
+ return True
+ else:
+ return False
+
+class TestHiroCollision(TestHiro):
+
+ @classmethod
+ def setUpClass(cls):
+ super(TestHiroCollision, cls).setUpClass()
+ cls.fout = open('/tmp/check-test_hironx_collision.txt', 'w')
+ cls.fout.write('Condition\tLoop\tComp Time\tRecov Time\tLoop for check\n')
+
+ cls.curr_loop = 1
+ try:
+ cls.robot.co_svc.setCollisionLoop(cls.curr_loop)
+ cls.use_set_collision_loop = True
+ except:
+ cls.use_set_collision_loop = False
+
+ cls.init_pose = [0]*29
+ # col_safe_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,0.15708,-0.113446,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0]
+ # col_fail_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.845363,0.03992,0.250074,-1.32816,0.167513,0.016204,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0]
+ cls.col_safe_pose = [0.0, 0.0, 0.0,
+ 0.0, -0.010471975511965976, 0.0,
+ -1.7453292519943295, 0.26529004630313807, 0.16406094968746698,
+ 0.05585053606381855, 0.0, 0.0,
+ 0.0, 0.0, 0.010471975511963548,
+ 0.0, -1.745329251994327, -0.265290046303138,
+ 0.16406094968746654, -0.055850536063817735, 0.0,
+ 0.0, 0.0, 0.0]
+ cls.col_fail_pose = [0.0, 0.0, 0.0,
+ 0.0, -0.010471975511965976, 0.0,
+ -1.7453292519943295, 0.26529004630313807, 0.16406094968746698,
+ 0.05585053606381855, 0.0, 0.0,
+ 0.0, 0.0, -0.9357196648099176,
+ -0.5020351200853644, -0.7480480183116466, -0.15644273591164157,
+ -0.10807458370637157, 0.9688350378358652, 0.0,
+ 0.0, 0.0, 0.0]
+ print >> sys.stderr, "hrpsys_version = %s" % cls.robot.hrpsys_version
+
+ def printCollisionState(self, cs):
+ print >> sys.stderr, "Collision State:"
+ print >> sys.stderr, "\tTime: %f" % cs.time
+ print >> sys.stderr, "\tComputation time: %f" % cs.computation_time
+ print >> sys.stderr, "\tSafe Posture: %s" % cs.safe_posture
+ print >> sys.stderr, "\tRecover time: %f" % cs.recover_time
+ print >> sys.stderr, "\tLoop for check: %d" % cs.loop_for_check
+ # print >> sys.stderr, cs.angle
+ # print >> sys.stderr, cs.collide
+ # print >> sys.stderr, cs.lines
+
+ def outputCollisionState(self, cs, condition):
+ s = "%.1f\t\t\t%d\t\t%f\t%f\t%d\n" % (condition, self.curr_loop, cs.computation_time, cs.recover_time, cs.loop_for_check)
+ self.fout.write(s)
+ self.fout.flush()
+
+ # demo functions
+ def demoCollisionCheckSafe (self):
+ print >> sys.stderr, "1. CollisionCheck in safe pose"
+ self.robot.seq_svc.setJointAngles(self.col_safe_pose, 3.0);
+ self.robot.waitInterpolation();
+ counter = 0
+ while (counter < 20) and (not vector_equal_eps([x / 180 * math.pi for x in self.robot.getJointAngles()], self.col_safe_pose)):
+ time.sleep(0.2)
+ counter = counter + 1
+ # print >> sys.stderr, counter
+ assert(counter != 20)
+ cs = self.robot.co_svc.getCollisionStatus()[1]
+ if cs.safe_posture:
+ print >> sys.stderr, " => Safe pose"
+ self.outputCollisionState(cs, 1)
+ self.printCollisionState(cs)
+ assert(cs.safe_posture is True)
+
+ def demoCollisionCheckFail (self):
+ print >> sys.stderr, "2. CollisionCheck in fail pose"
+ self.robot.seq_svc.setJointAngles(self.col_fail_pose, 3.0);
+ self.robot.waitInterpolation();
+ cs = self.robot.co_svc.getCollisionStatus()[1]
+ if not cs.safe_posture:
+ print >> sys.stderr, " => Successfully stop fail pose"
+ self.outputCollisionState(cs, 2.0)
+ self.printCollisionState(cs)
+ assert((not cs.safe_posture) is True)
+ self.robot.seq_svc.setJointAngles(self.col_safe_pose, 3.0);
+ self.robot.waitInterpolation();
+ cs=self.robot.co_svc.getCollisionStatus()[1]
+ if cs.safe_posture:
+ print >> sys.stderr, " => Successfully return to safe pose"
+ self.outputCollisionState(cs, 2.1)
+ self.printCollisionState(cs)
+ assert(cs.safe_posture is True)
+
+ def demoCollisionCheckFailWithSetTolerance (self):
+ print >> sys.stderr, "3. CollisionCheck in fail pose with 0.1[m] tolerance"
+ self.robot.co_svc.setTolerance("all", 0.1); # [m]
+ self.robot.seq_svc.setJointAngles(self.col_fail_pose, 1.0);
+ self.robot.waitInterpolation();
+ cs = self.robot.co_svc.getCollisionStatus()[1]
+ if not cs.safe_posture:
+ print >> sys.stderr, " => Successfully stop fail pose (0.1[m] tolerance)"
+ self.outputCollisionState(cs, 3.0)
+ self.printCollisionState(cs)
+ if StrictVersion(self.robot.hrpsys_version) >= StrictVersion('315.10.0'): # https://github.com/fkanehiro/hrpsys-base/pull/993 ???
+ assert((not cs.safe_posture) is True)
+ else:
+ print >> sys.stderr, " => WARNING Failed to stop fail pose (0.1[m] tolerance), because of https://github.com/fkanehiro/hrpsys-base/pull/993 ???"
+ self.robot.co_svc.setTolerance("all", 0.0); # [m]
+ self.robot.seq_svc.setJointAngles(self.col_safe_pose, 3.0);
+ self.robot.waitInterpolation();
+ cs = self.robot.co_svc.getCollisionStatus()[1]
+ if cs.safe_posture:
+ print >> sys.stderr, " => Successfully return to safe pose"
+ self.outputCollisionState(cs, 3.1)
+ self.printCollisionState(cs)
+ assert(cs.safe_posture is True)
+
+ def demoCollisionDisableEnable (self):
+ print >> sys.stderr, "4. CollisionDetection enable and disable"
+ self.robot.seq_svc.setJointAngles(self.col_safe_pose, 1.0);
+ self.robot.waitInterpolation();
+ if self.robot.co_svc.disableCollisionDetection():
+ print >> sys.stderr, " => Successfully disabled when no collision"
+ assert(self.robot.co_svc.disableCollisionDetection() is True)
+ if self.robot.co_svc.enableCollisionDetection():
+ print >> sys.stderr, " => Successfully enabled when no collision"
+ assert(self.robot.co_svc.enableCollisionDetection() is True)
+ self.robot.seq_svc.setJointAngles(self.col_fail_pose, 1.0);
+ self.robot.waitInterpolation();
+ if not self.robot.co_svc.disableCollisionDetection():
+ print >> sys.stderr, " => Successfully inhibit disabling when collision"
+ assert((not self.robot.co_svc.disableCollisionDetection()) is True)
+ self.robot.seq_svc.setJointAngles(self.col_safe_pose, 1.0);
+ self.robot.waitInterpolation();
+
+ def test_CollisionLoopChange(self):
+ import argparse
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--maxloop', action='store', type=int, nargs='?', default=3)
+ args,unknown=parser.parse_known_args()
+ if not self.use_set_collision_loop :
+ args.maxloop=1
+ for i in range(1, args.maxloop+1):
+ self.curr_loop = i
+ if self.use_set_collision_loop:
+ self.robot.co_svc.setCollisionLoop(self.curr_loop)
+ self.demoCollisionCheckSafe()
+ self.demoCollisionCheckFail()
+ self.demoCollisionCheckFailWithSetTolerance()
+ self.demoCollisionDisableEnable()
+
+ self.fout.close()
+
+if __name__ == '__main__':
+ import rostest
+ print("===================================================")
+ print("# Please consult test result with following process")
+ print("tail -f /tmp/check-test_hironx_collision.txt")
+ print("# You can run individual test with following command")
+ print("python -m unittest test_hironx_collision.TestHiroCollision.test_CollisionLoopChange")
+ rostest.rosrun(PKG, 'test_hronx_collision', TestHiroCollision)
diff --git a/hironx_ros_bridge/test/test_hironx_limb.py b/hironx_ros_bridge/test/test_hironx_limb.py
index 268f8bfe..d1127f6f 100755
--- a/hironx_ros_bridge/test/test_hironx_limb.py
+++ b/hironx_ros_bridge/test/test_hironx_limb.py
@@ -92,7 +92,7 @@ def test_rarm_setJointAngles_Clear (self):
data = self.load_log_data(q_filename)
print "check setJointAnglesOfGroup(clear) "+str(clear_time[i])
- self.check_log_data(data, 6, (5 + clear_time[i]), [(-140+i*40/len(clear_time)),20], -100.0)
+ self.check_log_data(data, 6, (5 + clear_time[i]), [(-140+i*40/len(clear_time)),20], -100.0, acc_thre = 0.1, tm_thre = 0.2)
def test_rarm_setJointAnglesOfGroup_Override_Acceleration (self):
self.limbbody_init()
@@ -233,6 +233,7 @@ def test_movejoints_neck_waist(self):
self.robot.goInitial()
self.assertTrue(self.robot.setTargetPoseRelative('torso', 'CHEST_JOINT0', dw=min_waist_yaw*safety_coeffiecient, tm=durtion_operation))
+# python -m unittest test_hironx_limb.TestHiroLimb.test_rarm_setJointAngles_Clear
if __name__ == '__main__':
import rostest
rostest.rosrun(PKG, 'test_hronx_limb', TestHiroLimb)
diff --git a/hironx_ros_bridge/test/test_hironx_target.py b/hironx_ros_bridge/test/test_hironx_target.py
index 04ccd85d..d40186eb 100755
--- a/hironx_ros_bridge/test/test_hironx_target.py
+++ b/hironx_ros_bridge/test/test_hironx_target.py
@@ -17,6 +17,7 @@
class TestHiroTarget(TestHiro):
def testSetTargetPoseBothArm(self):
+ print ";; testSetTargetPoseBothArm"
tm = 10
self.robot.goInitial()
posl1 = self.robot.getCurrentPosition('LARM_JOINT5')
@@ -50,6 +51,7 @@ def testSetTargetPoseBothArm(self):
assert(True)
def testGetReferencePose(self):
+ print ";; testGetReferencePose"
def print_pose(msg, pose):
print msg, (pose[3], pose[7], pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
self.robot.goInitial()
@@ -104,6 +106,7 @@ def print_pose(msg, pose):
numpy.testing.assert_array_almost_equal(numpy.array(rpy1), numpy.array(rpy2), decimal=2)
def testGetCurrentPose(self):
+ print ";; testGetCurrentPose"
def print_pose(msg, pose):
print msg, (pose[3], pose[7], pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
self.robot.goInitial()
@@ -168,6 +171,7 @@ def print_pose(msg, pose):
numpy.testing.assert_array_almost_equal(numpy.array(rpy1), numpy.array(rpy2), decimal=2)
def testGetterByFrame(self):
+ print ";; testGetterByFrame"
def print_pose(msg, pose):
print msg, (pose[3], pose[7], pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
@@ -254,7 +258,7 @@ def test_setTargetPoseRelative_rpy(self):
Test if with setTargetPoseRelative with RPY values the arm pose becomes as intended.
Contributed by Naoki Fuse (Daido Steel).
'''
-
+ print ";; test_setTargetPoseRelative_rpy"
print "goInitial", self.robot.getCurrentRPY('RARM_JOINT5'), self.robot.getCurrentRPY('LARM_JOINT5')
l_eef = 'LARM_JOINT5'
r_eef = 'RARM_JOINT5'
@@ -270,9 +274,11 @@ def test_setTargetPoseRelative_rpy(self):
# roll motion
self.robot.goInitial(2)
+ import time
for i in range(0, 5): # Repeat the same movement 5 times
+ print ";; roll motion ", i, "/4"
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dr=math.pi / 2, tm=0.5, wait=False)
- self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=math.pi / 2, tm=0.5, wait=True)
+ self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=math.pi / 2, tm=0.7, wait=True)
roll_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
roll_l_post_now = quaternion_from_euler(roll_l_post_now_rpy[0], roll_l_post_now_rpy[1], roll_l_post_now_rpy[2])
roll_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
@@ -280,7 +286,7 @@ def test_setTargetPoseRelative_rpy(self):
numpy.testing.assert_array_almost_equal(roll_l_post, roll_l_post_now, decimal=2)
numpy.testing.assert_array_almost_equal(roll_r_post, roll_r_post_now, decimal=2)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dr=-math.pi / 2, dw=0, tm=0.5, wait=False)
- self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=-math.pi / 2, dw=0, tm=0.5, wait=True)
+ self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=-math.pi / 2, dw=0, tm=0.7, wait=True)
init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
@@ -291,8 +297,9 @@ def test_setTargetPoseRelative_rpy(self):
# pitch motion
self.robot.goInitial(2)
for i in range(0, 5):
+ print ";; pitch motion ", i, "/4"
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dp=math.pi / 4, tm=0.5, wait=False)
- self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=math.pi / 4, tm=0.5, wait=True)
+ self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=math.pi / 4, tm=0.7, wait=True)
pitch_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
pitch_l_post_now = quaternion_from_euler(pitch_l_post_now_rpy[0], pitch_l_post_now_rpy[1], pitch_l_post_now_rpy[2])
pitch_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
@@ -300,7 +307,7 @@ def test_setTargetPoseRelative_rpy(self):
numpy.testing.assert_array_almost_equal(pitch_l_post, pitch_l_post_now, decimal=2)
numpy.testing.assert_array_almost_equal(pitch_r_post, pitch_r_post_now, decimal=2)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dp=-math.pi / 4, dw=0, tm=0.5, wait=False)
- self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=-math.pi / 4, dw=0, tm=0.5, wait=True)
+ self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=-math.pi / 4, dw=0, tm=0.7, wait=True)
init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
@@ -311,8 +318,9 @@ def test_setTargetPoseRelative_rpy(self):
# yaw motion
self.robot.goInitial(2)
for i in range(0, 5):
+ print ";; yaw motion ", i, "/4"
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dw=math.pi / 2, tm=0.5, wait=False)
- self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=math.pi / 2, tm=0.5, wait=True)
+ self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=math.pi / 2, tm=0.7, wait=True)
yaw_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
yaw_l_post_now = quaternion_from_euler(yaw_l_post_now_rpy[0], yaw_l_post_now_rpy[1], yaw_l_post_now_rpy[2])
yaw_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
@@ -320,7 +328,7 @@ def test_setTargetPoseRelative_rpy(self):
numpy.testing.assert_array_almost_equal(yaw_l_post, yaw_l_post_now, decimal=2)
numpy.testing.assert_array_almost_equal(yaw_r_post, yaw_r_post_now, decimal=2)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dw=-math.pi / 2, tm=0.5, wait=False)
- self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=-math.pi / 2, tm=0.5, wait=True)
+ self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=-math.pi / 2, tm=0.7, wait=True)
init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
@@ -339,9 +347,11 @@ def test_get_geometry_methods_noarg(self):
'getReferencePose', 'getReferencePosition',
'getReferenceRotation', 'getReferenceRPY']
'''
+ print ";; test_get_geometry_methods_noarg"
self.assertRaises(RuntimeError, lambda: self.robot.getCurrentPose())
self.assertRaises(RuntimeError, lambda: self.robot.getReferencePose())
+# python -m unittest test_hironx_target.TestHiroTarget.test_setTargetPoseRelative_rpy
if __name__ == '__main__':
import rostest
rostest.rosrun(PKG, 'test_hronx_target', TestHiroTarget)