def __init__(self, robot_description, base_link, arm_root_link, arm_tip_link, head_root_link, head_tip_link, arm_controller, head_controller, head_camera_frame, head_camera_joint, camera_namespace):
self.robot_description = robot_description
self.base_link = base_link
self.arm_root_link = arm_root_link
self.arm_tip_link = arm_tip_link
self.head_root_link = head_root_link
self.head_tip_link = head_tip_link
self.arm_controller = arm_controller
self.head_controller = head_controller
self.head_camera_frame = head_camera_frame
self.head_camera_joint = head_camera_joint
self.camera_namespace = camera_namespace
self.robot = URDF().parse(self.robot_description)
self.robot_name = self.robot.name.lower()
if not self.base_link:
self.base_link = self.robot.get_root()
if self.robot.link_map.has_key(self.base_link) == False:
self.error_tip_link(self.base_link, '--base-link')
if self.robot.link_map.has_key(self.arm_root_link) == False:
self.error_tip_link(self.arm_root_link, '--arm-root-link')
if self.robot.link_map.has_key(self.head_root_link) == False:
self.error_tip_link(self.head_root_link, '--head-root-link')
if self.robot.link_map.has_key(self.arm_tip_link) == False:
self.error_tip_link(self.arm_tip_link, '--arm-tip-link')
if self.robot.link_map.has_key(self.head_tip_link) == False:
self.error_tip_link(self.head_tip_link, '--head-tip-link')
if self.robot.link_map.has_key(self.head_camera_frame) == False:
self.error_tip_link(self.head_camera_frame, '--head-camera-frame')
if self.robot.joint_map.has_key(self.head_camera_joint) == False:
self.error_joint(self.head_camera_joint, '--head-camera-joint')
all_chain = []
for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]:
all_chain.append(self.robot.get_chain(base_link, end_link)[1:])
for c1,c2 in itertools.product(*all_chain):
if c1 == c2 :
rospy.logerr('arm/head chain share same joint ({}), this will cause failure'.format(c1))
sys.exit()
for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]:
joint_list = filter(lambda x: self.robot.joint_map.has_key(x) and self.robot.joint_map[x].type != 'fixed', [c for c in self.robot.get_chain(base_link, end_link)])
exec('self.{0}_joint_list = {1}'.format(limb, joint_list)) in locals()
rospy.loginfo('using following joint for {} chain'.format('arm'))
rospy.loginfo('... {}'.format(self.arm_joint_list))
rospy.loginfo('using following joint for {} chain'.format('head'))
rospy.loginfo('... {}'.format(self.head_joint_list))
# create robot_calibration directory
self.dirname_top = self.robot_name+'_calibration'
self.dirname_capture = self.robot_name+'_calibration/capture_data'
self.dirname_estimate = self.robot_name+'_calibration/estimate_params'
self.dirname_results = self.robot_name+'_calibration/view_results'
try:
os.makedirs(self.dirname_top)
os.makedirs(self.dirname_capture)
os.makedirs(self.dirname_estimate)
os.makedirs(self.dirname_results)
except Exception as e:
rospy.logfatal(e)
#sys.exit(-1)
# setup capture_data
self.write_all_pipelines(args.use_kinect) # all_pipelines.launch
self.write_all_viewers(args.use_kinect) # all_viewers.launch
self.write_capture_data() # capture_data.launch
self.write_capture_exec() # capture_exec.launch
self.write_hardware_config() # hardware_config
self.write_make_samples() # make_samples.py
# setup estimate_params
self.write_estimation_config() # estimation_config.launch
self.write_calibrate_sh() # calibrate_<robot>.sh
self.write_estimate_params_config(args.use_kinect) # free_arms.yaml free_cameras.yaml free_cb_locations.yaml free_torso.yaml system.yaml
# setup view_results
self.write_view_results() # scatter_config.yaml view_scatter.sh
def __init__(self, context):
super(BarrettDashboard, self).__init__(context)
self._joint_sub = None
# Give QObjects reasonable names
self.setObjectName('BarrettDashboard')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'barrett_dashboard.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('BarrettDashboardPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.context = context
jp_widgets = [getattr(self._widget, 'jp_%d' % i) for i in range(7)]
jn_widgets = [getattr(self._widget, 'jn_%d' % i) for i in range(7)]
self.joint_widgets = zip(jp_widgets, jn_widgets)
tp_widgets = [getattr(self._widget, 'tp_%d' % i) for i in range(7)]
tn_widgets = [getattr(self._widget, 'tn_%d' % i) for i in range(7)]
self.torque_widgets = zip(tp_widgets, tn_widgets)
self.joint_signals = []
self.torque_signals = []
for (tp, tn) in self.torque_widgets:
tp.setRange(0.0, 1.0, False)
tn.setRange(1.0, 0.0, False)
tp.setValue(0.0)
tn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#tp.setValue(v if v >=0 else 0)
#tn.setValue(-v if v <0 else 0)
for (jp, jn) in self.joint_widgets:
jp.setRange(0.0, 1.0, False)
jn.setRange(1.0, 0.0, False)
jp.setValue(0.0)
jn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#jp.setValue(v if v >=0 else 0)
#jn.setValue(-v if v <0 else 0)
self.barrett_green = QColor(87, 186, 142)
self.barrett_green_dark = self.barrett_green.darker()
self.barrett_green_light = self.barrett_green.lighter()
self.barrett_blue = QColor(80, 148, 204)
self.barrett_blue_dark = self.barrett_blue.darker()
self.barrett_blue_light = self.barrett_blue.lighter()
self.barrett_red = QColor(232, 47, 47)
self.barrett_red_dark = self.barrett_red.darker()
self.barrett_red_light = self.barrett_red.lighter()
self.barrett_orange = QColor(255, 103, 43)
self.barrett_orange_dark = self.barrett_orange.darker()
#background_color = p.mid().color()
joint_bg_color = self.barrett_blue_dark.darker()
joint_fill_color = self.barrett_blue
joint_alarm_color = self.barrett_blue #self.barrett_blue_light
torque_bg_color = self.barrett_green_dark.darker()
torque_fill_color = self.barrett_green
torque_alarm_color = self.barrett_orange #self.barrett_green_light
temp_bg_color = self.barrett_red_dark.darker()
temp_fill_color = self.barrett_orange
temp_alarm_color = self.barrett_red
for w in jp_widgets + jn_widgets:
w.setAlarmLevel(0.80)
w.setFillColor(joint_fill_color)
w.setAlarmColor(joint_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), joint_bg_color)
w.setPalette(p)
for w in tp_widgets + tn_widgets:
w.setAlarmLevel(0.66)
w.setFillColor(torque_fill_color)
w.setAlarmColor(torque_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), torque_bg_color)
w.setPalette(p)
self._widget.hand_temp.setAlarmLevel(0.66)
self._widget.hand_temp.setFillColor(temp_fill_color)
self._widget.hand_temp.setAlarmColor(temp_alarm_color)
p = self._widget.hand_temp.palette()
p.setColor(self._widget.hand_temp.backgroundRole(), temp_bg_color)
self._widget.hand_temp.setPalette(p)
self._widget.hand_temp.setOrientation(Qt.Horizontal,
QwtThermo.RightScale)
self._widget.jf_0.setStyleSheet(
"QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }"
% (joint_bg_color.red(), joint_bg_color.green(),
joint_bg_color.blue(), joint_fill_color.red(),
joint_fill_color.green(), joint_fill_color.blue()))
self.urdf = rospy.get_param('robot_description')
self.robot = URDF()
self.robot = self.robot.from_xml_string(self.urdf)
self.pos_norm = [0] * 7
self.torque_norm = [0] * 7
self._joint_sub = rospy.Subscriber('joint_states',
sensor_msgs.msg.JointState,
self._joint_state_cb)
self._status_sub = rospy.Subscriber('status',
oro_barrett_msgs.msg.BarrettStatus,
self._status_cb)
self._hand_status_sub = rospy.Subscriber(
'hand/status', oro_barrett_msgs.msg.BHandStatus,
self._hand_status_cb)
self._update_status(-1)
self.safety_mode = -1
self.run_mode = 0
self.hand_run_mode = 0
self.hand_min_temperature = 40.0
self.hand_max_temperature = 65.0
self.hand_temperature = 0.0
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widget_values)
self.update_timer.start()
self.set_home_client = actionlib.SimpleActionClient(
'wam/set_home_action', oro_barrett_msgs.msg.SetHomeAction)
self.set_mode_client = actionlib.SimpleActionClient(
'wam/set_mode_action', oro_barrett_msgs.msg.SetModeAction)
self._widget.button_set_home.clicked[bool].connect(
self._handle_set_home_clicked)
self._widget.button_idle_wam.clicked[bool].connect(
self._handle_idle_wam_clicked)
self._widget.button_run_wam.clicked[bool].connect(
self._handle_run_wam_clicked)
self.bhand_init_client = actionlib.SimpleActionClient(
'hand/initialize_action', oro_barrett_msgs.msg.BHandInitAction)
self.bhand_set_mode_client = actionlib.SimpleActionClient(
'hand/set_mode_action', oro_barrett_msgs.msg.BHandSetModeAction)
self.grasp_client = actionlib.SimpleActionClient(
'grasp', BHandGraspAction)
self.release_client = actionlib.SimpleActionClient(
'release', BHandReleaseAction)
self.spread_client = actionlib.SimpleActionClient(
'spread', BHandSpreadAction)
self._widget.button_initialize_hand.clicked[bool].connect(
self._handle_initialize_hand_clicked)
self._widget.button_idle_hand.clicked[bool].connect(
self._handle_idle_hand_clicked)
self._widget.button_run_hand.clicked[bool].connect(
self._handle_run_hand_clicked)
self._widget.button_release_hand.clicked[bool].connect(
self._handle_release_hand_clicked)
self._widget.button_grasp_hand.clicked[bool].connect(
self._handle_grasp_hand_clicked)
self._widget.button_set_spread.clicked[bool].connect(
self._handle_spread_hand_clicked)
self._widget.resizeEvent = self._handle_resize
def __init__(self, config_dir, system, robot_desc, output_debug=False):
# Load configs
self.cam_config = yaml.load(open(config_dir + "/cam_config.yaml"))
self.chain_config = yaml.load(open(config_dir + "/chain_config.yaml"))
self.laser_config = yaml.load(open(config_dir + "/laser_config.yaml"))
self.controller_config = yaml.load(
open(config_dir + "/controller_config.yaml"))
# Not all robots have lasers.... :(
if self.laser_config == None:
self.laser_config = dict()
# Debug mode makes bag files huge...
self.output_debug = output_debug
# Error message from sample capture
self.message = None
# Status message from interval computation
self.interval_status = None
# parse urdf and get list of links
links = URDF().parse(robot_desc).link_map.keys()
# load system config
system = yaml.load(open(system))
# remove cams that are not on urdf
for cam in self.cam_config.keys():
try:
link = system['sensors']['rectified_cams'][cam]['frame_id']
if not link in links:
print 'URDF does not contain link [%s]. Removing camera [%s]' % (
link, cam)
del self.cam_config[cam]
except:
print 'System description does not contain camera [%s]' % cam
del self.cam_config[cam]
# remove arms that are not on urdf
for chain in self.chain_config.keys():
try:
link = system['sensors']['chains'][chain]['tip']
if not link in links:
print 'URDF does not contain link [%s]. Removing chain [%s]' % (
link, chain)
del self.chain_config[chain]
except:
print 'System description does not contain chain [%s]' % chain
del self.chain_config[chain]
self.cache = RobotMeasurementCache()
self.lock = threading.Lock()
# Specifies if we're currently waiting for a sample
self.active = False
# Construct Configuration Manager (manages configuration of nodes in the pipeline)
self.config_manager = ConfigManager(self.cam_config, self.chain_config,
self.laser_config,
self.controller_config)
# Construct a manager for each sensor stream (Don't enable any of them)
self.cam_managers = [(cam_id,
CamManager(cam_id, self.add_cam_measurement))
for cam_id in self.cam_config.keys()]
self.chain_managers = [(chain_id,
ChainManager(chain_id,
self.add_chain_measurement))
for chain_id in self.chain_config.keys()]
self.laser_managers = [(laser_id,
LaserManager(laser_id,
self.add_laser_measurement))
for laser_id in self.laser_config.keys()]
# Subscribe to topic containing stable intervals
self.request_interval_sub = rospy.Subscriber(
"intersected_interval", calibration_msgs.msg.Interval,
self.request_callback)
# Subscribe to topic containing stable intervals
self.interval_status_sub = rospy.Subscriber(
"intersected_interval_status", calibration_msgs.msg.IntervalStatus,
self.status_callback)
#!/usr/bin/env python
import rospy
import actionlib
import sensor_msgs.msg
from urdf_parser_py.urdf import URDF
robot = URDF()
rate = None
def state_cb(msg):
joint_pos_map = dict(zip(msg.name, msg.position))
warn = False
for joint in robot.joints:
if joint.name in joint_pos_map:
pos = joint_pos_map[joint.name]
lower_dist = abs(pos - joint.limit.lower)
upper_dist = abs(joint.limit.upper - pos)
if lower_dist < 0.1:
if lower_dist < 0.01:
rospy.logerr("Joint %s is at lower limit!" % (joint.name))
else:
rospy.logwarn(
"Joint %s is within %0.3f rad of lower limit!" %
(joint.name, lower_dist))
