def setUpClass(self):
self._ros = ROS_Client()
self._botharms_joints = [
'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3',
'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1',
'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'
]
self._ros.MG_RARM_current_pose = self._ros.MG_RARM.get_current_pose(
).pose
self._ros.MG_LARM_current_pose = self._ros.MG_LARM.get_current_pose(
).pose
# For botharms get_current_pose ends with no eef error.
self.init_rtm_jointvals = [
0.010471975511965976, 0.0, -1.7453292519943295,
-0.26529004630313807, 0.16406094968746698, -0.05585053606381855,
-0.010471975511965976, 0.0, -1.7453292519943295,
0.26529004630313807, 0.16406094968746698, 0.05585053606381855
]
self.init_rtm_jointvals_factory = [
-1.3877787807814457e-17, 1.0842021724855044e-19,
-2.2689280275926285, -4.440892098500626e-16,
-2.220446049250313e-16, 0.0, 0.0, 1.0842021724855044e-19,
-2.2689280275926285, 2.220446049250313e-16,
-1.1102230246251565e-16, 5.551115123125783e-17
]
self.offpose_jointvals = [
0.0, 0.0, 0.0, -0.4363323129985819, -2.4260076602721163,
-2.7401669256310983, -0.7853981633974487, 0.0, 0.0,
0.4363323129985819, -2.4260076602721163, -2.7401669256310983,
0.7853981633974487, 0.0, 0.0
]
# These represent a pose as in the image https://goo.gl/hYa15h
self.banzai_pose_larm_goal = [
-0.0280391167993, 0.558512828409, 0.584801820449, 0.468552399035,
-0.546097642377, -0.449475560632, 0.529346516701
]
self.banzai_pose_rarm_goal = [
0.0765219167208, -0.527210000725, 0.638387081642, -0.185009037721,
-0.683111796219, 0.184872589841, 0.681873929223
]
self.banzai_jointvals_goal = [
1.3591412928962834, -1.5269810342586994, -1.5263864987632225,
-0.212938998306429, -0.19093239258017988, -1.5171864827145887,
-0.7066724299606867, -1.9314110634425135, -1.4268663042616962,
1.0613942164863952, 0.9037643195141568, 1.835342100423069
]
def __init__(self, rtm_robotname='HiroNX(Robot)0', url=''):
'''
Initiate both ROS and RTM robot clients, keep them public so that they
are callable from script. e.g. On ipython,
In [1]: acceptance.ros.go_init()
In [2]: acceptance.rtm.goOffPose()
'''
self._rtm_robotname = rtm_robotname
self._rtm_url = url
# Init RTM and ROS client.
self.ros = ROS_Client()
self._acceptance_ros_client = AcceptanceTestROS(self.ros)
if rospy.has_param('/gazebo'):
print(
"\033[32m[INFO] Assming Gazebo Siulator, so do not connect to CORBA systmes\033[0m"
)
print(
"\033[32m[INFO] run 'acceptance.run_tests_ros()' for ROS interface test\033[0m"
)
else:
self.rtm = HIRONX()
self.rtm.init(robotname=self._rtm_robotname, url=self._rtm_url)
self._acceptance_rtm_client = AcceptanceTestRTM(self.rtm)
print(
"\033[32m[INFO] run 'acceptance.run_tests_rtm()' for RTM interface test\033[0m"
)
def __init__(self, rtm_robotname='HiroNX(Robot)0', url=''):
'''
Initiate both ROS and RTM robot clients, keep them public so that they
are callable from script. e.g. On ipython,
In [1]: acceptance.ros.go_init()
In [2]: acceptance.rtm.goOffPose()
'''
self._rtm_robotname = rtm_robotname
self._rtm_url = url
# Init RTM and ROS client.
self.ros = ROS_Client()
self._acceptance_ros_client = AcceptanceTestROS(self.ros)
self.rtm = HIRONX()
self.rtm.init(robotname=self._rtm_robotname, url=self._rtm_url)
self._acceptance_rtm_client = AcceptanceTestRTM(self.rtm)
a_data = str(robot.getJointAngles())
self.pub.publish(a_data)
if __name__ == '__main__':
#-------------------------------------------initial_setting------------------------------------------
parser = argparse.ArgumentParser(description='NEXTAGE Open command line interpreters')
parser.add_argument('--host', help='corba name server hostname')
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()')
args, unknown = parser.parse_known_args()
if args.host:
rtm.nshost = args.host
if args.port:
rtm.nsport = args.port
if not args.robot:
args.robot = "RobotHardware0" if args.host else "HiroNX(Robot)0"
if not args.modelfile:
args.modelfile = ""
if len(unknown) >= 2:
args.robot = unknown[0]
args.modelfile = unknown[1]
robot = nxc = nextage_client.NextageClient()
robot.init(robotname=args.robot, url=args.modelfile)
ros = ROS_Client()
#--------------------------------------------end_initial_setting------------------------------------------
# 主処理
Tele()
class TestHironxMoveit(unittest.TestCase):
def __init__(self, *args, **kwargs):
super(TestHironxMoveit, self).__init__(*args, **kwargs)
@classmethod
def setUpClass(self):
self._ros = ROS_Client()
self._botharms_joints = [
'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3',
'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1',
'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'
]
self._ros.MG_RARM_current_pose = self._ros.MG_RARM.get_current_pose(
).pose
self._ros.MG_LARM_current_pose = self._ros.MG_LARM.get_current_pose(
).pose
# For botharms get_current_pose ends with no eef error.
self.init_rtm_jointvals = [
0.010471975511965976, 0.0, -1.7453292519943295,
-0.26529004630313807, 0.16406094968746698, -0.05585053606381855,
-0.010471975511965976, 0.0, -1.7453292519943295,
0.26529004630313807, 0.16406094968746698, 0.05585053606381855
]
self.init_rtm_jointvals_factory = [
-1.3877787807814457e-17, 1.0842021724855044e-19,
-2.2689280275926285, -4.440892098500626e-16,
-2.220446049250313e-16, 0.0, 0.0, 1.0842021724855044e-19,
-2.2689280275926285, 2.220446049250313e-16,
-1.1102230246251565e-16, 5.551115123125783e-17
]
self.offpose_jointvals = [
0.0, 0.0, 0.0, -0.4363323129985819, -2.4260076602721163,
-2.7401669256310983, -0.7853981633974487, 0.0, 0.0,
0.4363323129985819, -2.4260076602721163, -2.7401669256310983,
0.7853981633974487, 0.0, 0.0
]
# These represent a pose as in the image https://goo.gl/hYa15h
self.banzai_pose_larm_goal = [
-0.0280391167993, 0.558512828409, 0.584801820449, 0.468552399035,
-0.546097642377, -0.449475560632, 0.529346516701
]
self.banzai_pose_rarm_goal = [
0.0765219167208, -0.527210000725, 0.638387081642, -0.185009037721,
-0.683111796219, 0.184872589841, 0.681873929223
]
self.banzai_jointvals_goal = [
1.3591412928962834, -1.5269810342586994, -1.5263864987632225,
-0.212938998306429, -0.19093239258017988, -1.5171864827145887,
-0.7066724299606867, -1.9314110634425135, -1.4268663042616962,
1.0613942164863952, 0.9037643195141568, 1.835342100423069
]
def _set_target_random(self):
'''
@type self: moveit_commander.MoveGroupCommander
@param self: In this particular test script, the argument "self" is
either 'rarm' or 'larm'.
'''
global current, current2, target
current = self.get_current_pose()
print "*current*", current
target = self.get_random_pose()
print "*target*", target
self.set_pose_target(target)
self.go()
current2 = self.get_current_pose()
print "*current2*", current2
# Associate a method to MoveGroupCommander class. This enables users to use
# the method on interpreter.
# Although not sure if this is the smartest Python code, this works fine from
# Python interpreter.
# #MoveGroupCommander._set_target_random = _set_target_random
# ****** Usage ******
#
# See wiki tutorial
# https://code.google.com/p/rtm-ros-robotics/wiki/hironx_ros_bridge_en#Manipulate_Hiro_with_Moveit_via_Python
def test_set_pose_target_rpy(self):
# #rpy ver
target = [0.2035, -0.5399, 0.0709, 0, -1.6, 0]
self._ros.MG_RARM.set_pose_target(target)
self._ros.MG_RARM.plan()
self.assertTrue(self._ros.MG_RARM.go())
def test_set_pose_target_quarternion(self):
target = [
0.2035, -0.5399, 0.0709, 0.000427, 0.000317, -0.000384, 0.999999
]
self._ros.MG_RARM.set_pose_target(target)
self._ros.MG_RARM.plan()
self.assertTrue(self._ros.MG_RARM.go())
def test_botharms_compare_joints(self):
'''Comparing joint names.'''
self.assertItemsEqual(self._ros.MG_BOTHARMS.get_joints(),
self._botharms_joints)
def test_botharms_get_joint_values(self):
'''
"both_arm" move group can't set pose target for now (July, 2015) due to
missing eef in the moveit config. Here checking if
MoveGroup.get_joint_values is working.
'''
self._ros.MG_LARM.set_pose_target(self.banzai_pose_larm_goal)
self._ros.MG_LARM.plan()
self._ros.MG_LARM.go()
self._ros.MG_RARM.set_pose_target(self.banzai_pose_rarm_goal)
self._ros.MG_RARM.plan()
self._ros.MG_RARM.go()
# Comparing to the 3rd degree seems too aggressive; example output:
# x: array([ 1.35906843, -1.52695742, -1.52658066, -0.21309358,
# -0.19092542, -1.51707957, -0.70651268, -1.93170852,
# -1.42660669, 1.0629058, 0.90412021, 1.83650476])
# y: array([ 1.35914129, -1.52698103, -1.5263865 , -0.212939,
# -0.19093239, -1.51718648, -0.70667243, -1.93141106,
# -1.4268663 , 1.06139422, 0.90376432, 1.8353421 ])
numpy.testing.assert_almost_equal(
self._ros.MG_BOTHARMS.get_current_joint_values(),
self.banzai_jointvals_goal, 2)
def test_botharms_set_named_target(self):
'''
Test if moveit_commander.set_named_target brings the arms to
the init_rtm pose defined in HiroNx.srdf.
'''
# Move the arms to non init pose.
self._ros.MG_LARM.set_pose_target(self.banzai_pose_larm_goal)
self._ros.MG_LARM.plan()
self._ros.MG_LARM.go()
self._ros.MG_RARM.set_pose_target(self.banzai_pose_rarm_goal)
self._ros.MG_RARM.plan()
self._ros.MG_RARM.go()
self._ros.MG_BOTHARMS.set_named_target('init_rtm')
self._ros.MG_BOTHARMS.plan()
self._ros.MG_BOTHARMS.go()
# Raises AssertException when the assertion fails, which
# automatically be flagged false by unittest framework.
# http://stackoverflow.com/a/4319836/577001
numpy.testing.assert_almost_equal(
self._ros.MG_BOTHARMS.get_current_joint_values(),
self.init_rtm_jointvals, 3)
# def test_simple_unittest(self):
# self.assertEqual(1, 1)
def test_rosclient_robotcommander(self):
'''
Starting from https://github.com/start-jsk/rtmros_hironx/pull/422,
ROS_Client.py depends on moveit_commander.RobotCommander class.
This case tests the integration of ROS_Client.py and RobotCommander.
Developers need to avoid testing moveit_commander.RobotCommander itself
-- that needs to be done upstream.
'''
# If the list of movegroups are not none, that can mean
# RobotCommander is working as expected.
groupnames = self._ros.get_group_names()
self.assertIsNotNone(groupnames)
def test_rosclient_goInitial(self):
self._ros.goInitial()
numpy.testing.assert_almost_equal(
self._ros.MG_BOTHARMS.get_current_joint_values(),
self.init_rtm_jointvals, 3)
self._ros.goInitial(init_pose_type=1)
numpy.testing.assert_almost_equal(
self._ros.MG_BOTHARMS.get_current_joint_values(),
self.init_rtm_jointvals_factory, 3)
def test_rosclient_go_offpose(self):
self._ros.go_offpose()
numpy.testing.assert_almost_equal(
self._ros.MG_UPPERBODY.get_current_joint_values(),
self.offpose_jointvals, 3)
def test_geometry_link_r4_r5(self):
'''
Trying to capture issues like https://github.com/tork-a/rtmros_nextage/issues/244
'''
TRANS_RARM4_RARM5 = [-0.047, 0.000, -0.090] # Expected value.
tflistener = tf.TransformListener()
(trans, rot) = tflistener.lookupTransform("/RARM_JOINT5_Link",
"/RARM_JOINT4_Link",
rospy.Time(0))
[
self.assertAlmostEqual(v_trans, v_expected, 3)
for v_trans, v_expected in zip(trans, TRANS_RARM4_RARM5)
]