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, robotname="HiroNX(Robot)0", url=""):
'''
Calls init from its superclass, which tries to connect RTCManager,
looks for ModelLoader, and starts necessary RTC components. Also runs
config, logger.
Also internally calls setSelfGroups().
@type robotname: str
@type url: str
'''
HIRONX.init(self, robotname=robotname, url=url)
def goInitial(self, tm=7, wait=True, init_pose_type=0):
'''
@see: HIRONX.goInitial
'''
if not init_pose_type:
# Set the pose where eefs level with the tabletop by default.
init_pose_type = HIRONX.INITPOS_TYPE_EVEN
return HIRONX.goInitial(self, tm, wait, init_pose_type)
def __init__(self, parent, guicontext):
'''
A GUi panel to list common operation commands for Hironx / NEXTAGE Open robot.
@param guicontext: The plugin context to create the monitor in.
@type guicontext: qt_gui.plugin_context.PluginContext
'''
super(HironxoCommandPanel, self).__init__()
self._parent = parent
self._guicontext = guicontext
# RTM Client
rtm.nshost = self.get_rosmaster_domain().hostname
rtm.nsport = rospy.get_param('rtmnameserver_port', '15005')
robotname = rospy.get_param('rtmnameserver_robotname',
'HiroNX(Robot)0')
rospy.loginfo(
'Connecting to RTM nameserver. host={}, port={}, robotname={}'.
format(rtm.nshost, rtm.nsport, robotname))
self._rtm = HIRONX()
self._rtm.init(robotname=robotname, url='')
rospack = RosPack()
ui_file = os.path.join(rospack.get_path(PKG_NAME), 'resource',
'hironx_commandpanel_main.ui')
loadUi(ui_file, self, {'HironxoCommandPanel': HironxoCommandPanel})
self._precision_output = 4 # Default degree order to be used to print values
# Assign callback methods
self.pushButton_checkEncoders.clicked[bool].connect(
self._check_encoders)
self.pushButton_goInitial.clicked[bool].connect(self._go_initial)
self.pushButton_goInitial_factoryval.clicked[bool].connect(
self._go_initial_factoryval)
self.pushButton_goOffPose.clicked[bool].connect(self._go_offPose)
self.pushButton_startImpedance.clicked[bool].connect(
self._impedance_on)
self.pushButton_stopImpedance.clicked[bool].connect(
self._impedance_off)
self.pushButton_actualPose_l.clicked[bool].connect(self._actual_pose_l)
self.pushButton_actualPose_r.clicked[bool].connect(self._actual_pose_r)
self.spinBox_precision_output.valueChanged[int].connect(
self._get_precision_output)
self.pushButton_groups.clicked[bool].connect(self._show_groups)
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)
def getRTCList(self):
'''
Overwriting HrpsysConfigurator.getRTCList
Returning predefined list of RT components.
As of March 2016, this method internally calls HIRONX.getRTCList() and
returns what it returns. Although we could simply remove this method
from NextageClient, we still keep it; it'd be easier
to modify an existing method than to add a new overridden method,
in case we might want to define RTC return list differently from HIRONX.
@rtype [[str]]
@return List of available components. Each element consists of a list
of abbreviated and full names of the component.
'''
return HIRONX.getRTCList(self)
class HironxoCommandPanel(QWidget):
'''
Design decisions:
- joint_state_publisher is dropped, since hrpsys_ros_bridge seems to set
`robot_description` param using COLLADA (see https://goo.gl/aLpILa)
instead of URDF, which joint_state_publisher is not capable of.
'''
def __init__(self, parent, guicontext):
'''
A GUi panel to list common operation commands for Hironx / NEXTAGE Open robot.
@param guicontext: The plugin context to create the monitor in.
@type guicontext: qt_gui.plugin_context.PluginContext
'''
super(HironxoCommandPanel, self).__init__()
self._parent = parent
self._guicontext = guicontext
# RTM Client
rtm.nshost = rospy.get_param('rtmnameserver_host', 'localhost')
rtm.nsport = rospy.get_param('rtmnameserver_port', '15005')
robotname = rospy.get_param('rtmnameserver_robotname', 'HiroNX(Robot)0')
rospy.loginfo('Connecting to RTM nameserver. host={}, port={}, robotname={} using rtmnameserver_{{host,port,robotname}} ROS param'.format(rtm.nshost, rtm.nsport, robotname))
self._rtm = HIRONX()
self._rtm.init(robotname=robotname, url='')
rospack = RosPack()
ui_file = os.path.join(rospack.get_path(PKG_NAME), 'resource',
'hironx_commandpanel_main.ui')
loadUi(ui_file, self, {'HironxoCommandPanel': HironxoCommandPanel})
self._precision_output = 4 # Default degree order to be used to print values
# Assign callback methods
self.pushButton_checkEncoders.clicked[bool].connect(self._check_encoders)
self.pushButton_goInitial.clicked[bool].connect(self._go_initial)
self.pushButton_goInitial_factoryval.clicked[bool].connect(self._go_initial_factoryval)
self.pushButton_goOffPose.clicked[bool].connect(self._go_offPose)
self.pushButton_startImpedance.clicked[bool].connect(self._impedance_on)
self.pushButton_stopImpedance.clicked[bool].connect(self._impedance_off)
self.pushButton_actualPose_l.clicked[bool].connect(self._actual_pose_l)
self.pushButton_actualPose_r.clicked[bool].connect(self._actual_pose_r)
self.spinBox_precision_output.valueChanged[int].connect(self._get_precision_output)
self.pushButton_groups.clicked[bool].connect(self._show_groups)
def get_rosmaster_domain(self):
'''
Workaround for rosgraph.Master.getUri() that does NOT return
a domain name with ".local".
'''
master = Master('/hironxo_command_widget')
#masteruri_http = master.getUri() # This does not obtain a hostname with ".local",
# regardless ROS_MASTER_URI actually contains it.
masteruri_http = os.environ['ROS_MASTER_URI']
urlparsed = urlparse(masteruri_http)
return urlparsed
def _print_command(self, command_str):
self.textBrowser_output.append('***Command used***\n\t' + command_str)
def _get_duration(self):
'''
@rtype float
'''
return float(self.doubleSpinBox_duration.text())
def _get_precision_output(self):
self._precision_output = self.spinBox_precision_output.value()
def _get_arm_impedance(self):
'''
@rtype str
@return: Returns a name of arm group that is checked on GUI. None by default.
'''
checked_arm = None
if self.radioButton_impedance_left.isChecked():
checked_arm = 'larm'
elif self.radioButton_impedance_right.isChecked():
checked_arm = 'rarm'
return checked_arm
def _check_encoders(self):
self._print_command('checkEncoders()')
self._rtm.checkEncoders()
def _go_initial(self):
self._print_command('goInitial(tm={})'.format(self._get_duration()))
self._rtm.goInitial(tm=self._get_duration())
def _go_initial_factoryval(self):
self._print_command('goInitial(init_pose_type=1, tm={})'.format(self._get_duration()))
self._rtm.goInitial(init_pose_type=self._rtm.INITPOS_TYPE_FACTORY,
tm=self._get_duration())
def _go_offPose(self):
self._print_command('goOffPose(tm={})'.format(self._get_duration()))
self._rtm.goOffPose(tm=self._get_duration())
def _impedance_on_off(self, do_on=True):
'''
Start/stop impedance control for the specified arm group.
Arm group to operate impedance control is automatically obtained from
GUI internally within this method.
@raise AttributeError: When no arm group is specified.
@type do_on: bool
@param do_on: On/off of impedance control
'''
armgroup = self._get_arm_impedance()
if not armgroup:
raise AttributeError('No arm is specified for impedance control to start.')
if do_on:
self._print_command('startImpedance({})'.format(armgroup))
self._rtm.startImpedance(armgroup)
elif not do_on:
self._print_command('stopImpedance({})'.format(armgroup))
self._rtm.stopImpedance(armgroup)
def _impedance_on(self):
self._impedance_on_off()
def _impedance_off(self):
self._impedance_on_off(do_on=False)
def _show_actual_pose(self, list_pose):
'''
@type list_pose: [str]
@param list_pose: list of str that represent angles (in radian)
'''
# Print the section line. This creates '---- ---- ---- ---- '
section_line_piece = '-'
text_single_line = section_line_piece * self._precision_output + '\t'
self.textBrowser_output.append(text_single_line * 4)
text_single_line = ''
i = 0
for fl in list_pose:
val = str(round(fl, self._precision_output))
# Format the diagonal
text_single_line += val
print('DEBUG) #{}: text_single_line: {}'.format(i, text_single_line))
# If num of elements in a single line reaches 4,
# or if cursor reaches the end of the list.
# Also, We want to add the 1st element into the 1st line.
if (i != 0 and i % 4 == 3) or i+1 == len(list_pose):
self.textBrowser_output.append(text_single_line)
text_single_line = '' # Clear the text for a single line
else:
text_single_line += '\t'
i += 1
def _actual_pose_l(self):
target_joint = 'LARM_JOINT5'
self._print_command('getCurrentPose({})'.format(target_joint))
self._show_actual_pose(self._rtm.getCurrentPose(target_joint))
def _actual_pose_r(self):
target_joint = 'LARM_JOINT5'
self._print_command('getCurrentPose({})'.format(target_joint))
self._show_actual_pose(self._rtm.getCurrentPose(target_joint))
def _show_groups(self):
groups = self._rtm.Groups
text = ''
for g in groups:
text += g[0]
str_elems = ''.join(str('\t' + e + '\n') for e in g[1])
text += str_elems
self._print_command('Groups')
self.textBrowser_output.append(text)
def init(self, robotname='HiroNX(Robot)0', url=''):
self.robot = hiro = HIRONX()
self.robot.init(robotname=args.robot, url=args.modelfile)
class AcceptanceTest_Hiro():
'''
This class holds methods that can be used for verification of the robot
Kawada Industries' dual-arm robot Hiro (and the same model of other robots
e.g. NEXTAGE OPEN).
This test class is:
- Intended to be run as nosetests, ie. roscore isn't available by itself.
- Almost all the testcases don't run without an access to a robot running.
Above said, combined with a rostest that launches roscore and robot (either
simulation or real) hopefully these testcases can be run, both in batch
manner by rostest and in nosetest manner.
Prefix for methods 'at' means 'acceptance test'.
All time arguments are second.
'''
_POSITIONS_LARM_DEG_UP = [
-4.697, -2.012, -117.108, -17.180, 29.146, -3.739
]
_POSITIONS_LARM_DEG_DOWN = [
6.196, -5.311, -73.086, -15.287, -12.906, -2.957
]
_POSITIONS_RARM_DEG_DOWN = [-4.949, -3.372, -80.050, 15.067, -7.734, 3.086]
_POSITIONS_LARM_DEG_UP_SYNC = [
-4.695, -2.009, -117.103, -17.178, 29.138, -3.738
]
_POSITIONS_RARM_DEG_UP_SYNC = [
4.695, -2.009, -117.103, 17.178, 29.138, 3.738
]
_ROTATION_ANGLES_HEAD_1 = [[0.1, 0.0], [50.0, 10.0]]
_ROTATION_ANGLES_HEAD_2 = [[50, 10], [-50, -10], [0, 0]]
_POSITIONS_TORSO_DEG = [[130.0], [-130.0]]
_R_Z_SMALLINCREMENT = 0.0001
# Workspace; X near, Y far
_POS_L_X_NEAR_Y_FAR = [0.326, 0.474, 1.038]
_RPY_L_X_NEAR_Y_FAR = (-3.075, -1.569, 3.074)
_POS_R_X_NEAR_Y_FAR = [0.326, -0.472, 1.048]
_RPY_R_X_NEAR_Y_FAR = (3.073, -1.569, -3.072)
# Workspace; X far, Y far
_POS_L_X_FAR_Y_FAR = [
0.47548142379781055, 0.17430276793604782, 1.0376878025614884
]
_RPY_L_X_FAR_Y_FAR = (-3.075954857224205, -1.5690261926181046,
3.0757659493049574)
_POS_R_X_FAR_Y_FAR = [
0.4755337947019357, -0.17242322190721648, 1.0476395479774052
]
_RPY_R_X_FAR_Y_FAR = (3.0715850722714944, -1.5690204449882248,
-3.071395243174742)
_TASK_DURATION_DEFAULT = 5.0
_DURATION_EACH_SMALLINCREMENT = 0.1
_TASK_DURATION_TORSO = 4.0
_TASK_DURATION_HEAD = 2.5
_DURATON_TOTAL_SMALLINCREMENT = 30 # Second.
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 _wait_input(self, msg, do_wait_input=False):
'''
@type msg: str
@param msg: To be printed on prompt.
@param do_wait_input: If True python commandline waits for any input
to proceed.
'''
if msg:
msg = '\n' + msg + '= '
if do_wait_input:
try:
input('Waiting for any key. **Do NOT** hit enter multiple ' +
'times. ' + msg)
except NameError:
# On python < 3, `input` can cause errors like following with
# any input string. So here just catch-release it.
# e.g.
# NameError: name 'next' is not defined
pass
def run_tests_rtm(self, do_wait_input=False):
'''
@param do_wait_input: If True, the user will be asked to hit any key
before each task to proceed.
'''
self._run_tests(self._acceptance_rtm_client, do_wait_input)
def run_tests_ros(self, do_wait_input=False):
'''
Run by ROS exactly the same actions that run_tests_rtm performs.
@param do_wait_input: If True, the user will be asked to hit any key
before each task to proceed.
'''
self._run_tests(self._acceptance_ros_client, do_wait_input)
def _run_tests(self, test_client, do_wait_input=False):
'''
@type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest
'''
_task_msgs = [
'TASK-1-1. Move each arm separately to the given pose by passing joint space.',
'TASK-1-2. Move each arm separately to the given pose by specifying a pose.',
'TASK-2. Move both arms at the same time using relative distance and angle from the current pose.',
'TASK-3. Move arm with very small increment (0.1mm/sec).\n\tRight hand 3 cm upward over 30 seconds.',
'In the beginning you\'ll see the displacement of the previous task.'
+ '\nTASK-4. Move head using Joint angles in degree.',
'TASK-5. Rotate torso to the left and right 130 degree.',
'TASK-6. Overwrite ongoing action.' +
'\n\t6-1. While rotating torso toward left, it gets canceled and rotate toward right.'
+
'\n\t6-2. While lifting left hand, right hand also tries to reach the same height that gets cancelled so that it stays lower than the left hand.',
'TASK-7. Show the capable workspace on front side of the robot.'
]
_msg_type_client = None
if isinstance(test_client, AcceptanceTestROS):
_msg_type_client = '(ROS) '
elif isinstance(test_client, AcceptanceTestRTM):
_msg_type_client = '(RTM) '
test_client.go_initpos()
msg_task = _task_msgs[0]
msg = _msg_type_client + msg_task
self._wait_input(msg, do_wait_input)
self._move_armbyarm_jointangles(test_client)
msg_task = _task_msgs[1]
msg = _msg_type_client + msg_task
self._wait_input(msg, do_wait_input)
self._move_armbyarm_pose(test_client)
msg_task = _task_msgs[2]
msg = _msg_type_client + msg_task
self._wait_input(msg, do_wait_input)
self._movearms_together(test_client)
msg_task = _task_msgs[3]
msg = _msg_type_client + msg_task
self._wait_input(msg, do_wait_input)
self._set_pose_relative(test_client)
msg_task = _task_msgs[4]
msg = _msg_type_client + msg_task
self._wait_input(msg, do_wait_input)
self._move_head(test_client)
msg_task = _task_msgs[5]
msg = _msg_type_client + msg_task
self._wait_input(msg, do_wait_input)
self._move_torso(test_client)
msg_task = _task_msgs[6]
msg = _msg_type_client + msg_task
self._wait_input(msg, do_wait_input)
self._overwrite_torso(test_client)
self._overwrite_arm(test_client)
msg_task = _task_msgs[7]
msg = _msg_type_client + msg_task
self._wait_input(msg, do_wait_input)
self._show_workspace(test_client)
def _move_armbyarm_jointangles(self, test_client):
'''
@type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest
'''
test_client.go_initpos()
arm = Constant.GRNAME_LEFT_ARM
test_client.set_joint_angles(arm, self._POSITIONS_LARM_DEG_UP,
'Task1 {}'.format(arm))
arm = Constant.GRNAME_RIGHT_ARM
test_client.set_joint_angles(arm, self._POSITIONS_RARM_DEG_DOWN,
'Task1 {}'.format(arm))
time.sleep(2.0)
def _move_armbyarm_pose(self, test_client):
print('** _move_armbyarm_pose isn\'t yet implemented')
pass
def _movearms_together(self, test_client):
'''
@type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest
'''
test_client.go_initpos()
arm = Constant.GRNAME_LEFT_ARM
test_client.set_joint_angles(arm, self._POSITIONS_LARM_DEG_UP_SYNC,
'{}'.format(arm),
self._TASK_DURATION_DEFAULT, False)
#'task2; Under current implementation, left arm ' +
#'always moves first, followed immediately by right arm')
arm = Constant.GRNAME_RIGHT_ARM
test_client.set_joint_angles(arm, self._POSITIONS_RARM_DEG_DOWN,
'{}'.format(arm),
self._TASK_DURATION_DEFAULT, False)
time.sleep(2.0)
def _set_pose_relative(self, test_client):
test_client.go_initpos()
delta = self._R_Z_SMALLINCREMENT
t = self._DURATION_EACH_SMALLINCREMENT
t_total_sec = self._DURATON_TOTAL_SMALLINCREMENT
total_increment = int(t_total_sec / t)
msg_1 = ('Right arm is moving upward with' +
'{}mm increment per {}s'.format(delta, t))
msg_2 = ' (Total {}cm over {}s).'.format(delta * total_increment,
t_total_sec)
print(msg_1 + msg_2)
for i in range(total_increment):
msg_eachloop = '{}th loop;'.format(i)
test_client.set_pose_relative(Constant.GRNAME_RIGHT_ARM,
dz=delta,
msg_tasktitle=msg_eachloop,
task_duration=t,
do_wait=True)
def _move_head(self, test_client):
test_client.go_initpos()
for positions in self._ROTATION_ANGLES_HEAD_1:
test_client.set_joint_angles(Constant.GRNAME_HEAD, positions,
'(1);', self._TASK_DURATION_HEAD)
for positions in self._ROTATION_ANGLES_HEAD_2:
test_client.set_joint_angles(Constant.GRNAME_HEAD, positions,
'(2);', self._TASK_DURATION_HEAD)
def _move_torso(self, test_client):
test_client.go_initpos()
for positions in self._POSITIONS_TORSO_DEG:
test_client.set_joint_angles(Constant.GRNAME_TORSO, positions, '')
def _overwrite_torso(self, test_client):
test_client.go_initpos()
test_client.set_joint_angles(Constant.GRNAME_TORSO,
self._POSITIONS_TORSO_DEG[0], '(1)',
self._TASK_DURATION_TORSO, False)
time.sleep(2.0)
test_client.set_joint_angles(Constant.GRNAME_TORSO,
self._POSITIONS_TORSO_DEG[1], '(2)',
self._TASK_DURATION_TORSO, True)
time.sleep(2.0)
def _overwrite_arm(self, test_client):
test_client.go_initpos()
test_client.set_joint_angles(Constant.GRNAME_LEFT_ARM,
self._POSITIONS_LARM_DEG_UP_SYNC, '(1)',
self._TASK_DURATION_DEFAULT, False)
test_client.set_joint_angles(
Constant.GRNAME_RIGHT_ARM, self._POSITIONS_RARM_DEG_UP_SYNC,
'(2) begins. Overwrite previous arm command.' +
'\n\tIn the beginning both arm starts to move to the' +
'\n\tsame height, but to the left arm interrupting' +
'\ncommand is sent and it goes downward.',
self._TASK_DURATION_DEFAULT, False)
time.sleep(2.0)
test_client.set_joint_angles(Constant.GRNAME_LEFT_ARM,
self._POSITIONS_LARM_DEG_DOWN, '(3)',
self._TASK_DURATION_DEFAULT, False)
def _show_workspace(self, test_client):
test_client.go_initpos()
msg = '; '
larm = Constant.GRNAME_LEFT_ARM
rarm = Constant.GRNAME_RIGHT_ARM
# X near, Y far.
test_client.set_pose(larm, self._POS_L_X_NEAR_Y_FAR,
self._RPY_L_X_NEAR_Y_FAR, msg + '{}'.format(larm),
self._TASK_DURATION_DEFAULT, False)
test_client.set_pose(rarm, self._POS_R_X_NEAR_Y_FAR,
self._RPY_R_X_NEAR_Y_FAR, msg + '{}'.format(rarm),
self._TASK_DURATION_DEFAULT, True)
# X far, Y far.
test_client.set_pose(larm, self._POS_L_X_FAR_Y_FAR,
self._RPY_L_X_FAR_Y_FAR, msg + '{}'.format(larm),
self._TASK_DURATION_DEFAULT, False)
test_client.set_pose(rarm, self._POS_R_X_FAR_Y_FAR,
self._RPY_R_X_FAR_Y_FAR, msg + '{}'.format(rarm),
self._TASK_DURATION_DEFAULT, True)