class NoLimitJointControlDialog(Plugin):
updateStateSignal = Signal(object)
updateGhostSignal = Signal(object)
def __init__(self, context):
super(NoLimitJointControlDialog, self).__init__(context)
self.setObjectName('NoLimitJointControlDialog')
self.updateStateSignal.connect(self.on_updateState)
self.updateGhostSignal.connect(self.on_updateGhost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = QWidget()
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget();
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radioGroup.addButton(self.radio_ghost_target,0)
self.radio_ghost_target.setChecked(True)
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_robot_target,1)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
#hbox_radio.addWidget(QLabel("Ghost"))
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
#hbox_radio.addWidget(QLabel("Robot"))
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = NoLimitJointControl(self, roslib.packages.get_pkg_dir('vigir_rqt_no_limit_joint_control') + '/launch/joints.txt',hbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
print "Add buttons to apply all ..."
all_widget = QWidget()
all_box = QHBoxLayout()
self.snap_to_ghost_button = QPushButton("SnapAllGhost")
self.snap_to_ghost_button.pressed.connect(self.on_snapGhostPressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_applyRobotPressed)
all_box.addWidget(self.apply_to_robot_button)
self.apply_to_robot_button = QPushButton("Apply WBC Robot")
self.apply_to_robot_button.pressed.connect(self.on_applyWBCRobotPressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
# all_hbox = QHBoxLayout()
# all_hbox.addStretch()
# all_hbox.addWidget(all_widget)
# all_hbox.addStretch()
# bottom_widget=QWidget()
# bottom_widget.setLayout(all_jbox)
# vbox.addWidget(bottom_widget)
vbox.addStretch()
self._widget.setLayout(vbox)
context.add_widget(self._widget)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states',JointState, queue_size=10)
def shutdown_plugin(self):
#Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
print "Shutdown ..."
#rospy.sleep(0.1)
self.stateSubscriber.unregister()
self.ghostSubscriber.unregister()
self.wbc_robot_pub.unregister()
print "Done!"
def stateCallbackFnc(self, atlasState_msg):
self.updateStateSignal.emit(atlasState_msg)
def ghostCallbackFnc(self, ghost_joint_state_msg):
self.updateGhostSignal.emit(ghost_joint_state_msg)
def on_snapGhostPressed(self):
print "Snap all joint values to current ghost joint positions"
for controller in self.joint_control.controllers:
controller.on_snapGhostPressed()
def on_snapCurrentPressed(self):
print "Snap all joint values to current joint positions"
for controller in self.joint_control.controllers:
controller.on_snapCurrentPressed()
def on_applyRobotPressed(self):
print "Send all latest joint values directly to robot"
for controller in self.joint_control.controllers:
controller.on_applyRobotPressed()
def on_applyWBCRobotPressed(self):
print "Send all latest joint values directly to robot as a WBC command"
if self.first_time:
print "Uninitialized !"
else:
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = copy.deepcopy(self.joint_states.name)
joint_state.position = list(copy.deepcopy(self.joint_states.position))
joint_state.velocity = list(copy.deepcopy(self.joint_states.velocity))
#print "Positions: ",joint_state.position
for i,name in enumerate(joint_state.name):
# for each joint, retrieve the data from each controller to populate the WBC vectors
for controller in self.joint_control.controllers:
for joint in controller.joints:
if joint.name == self.joint_states.name[i]:
print " fetching value for "+name+" = "+str(joint.position)+" at ndx="+str(i)
joint_state.position[i] = joint.position
joint_state.velocity[i] = joint.velocity
#joint_state.effort[i] = joint.effort
self.wbc_robot_pub.publish(joint_state)
# this runs in the Qt GUI thread and can therefore update the GUI
def on_updateState(self, atlasState_msg):
self.joint_states = atlasState_msg
self.joint_control.updateJointPositions(self.joint_states,self.first_time)
if self.first_time:
self.joint_control.resetCurrentJointSliders()
self.first_time = False
self.ghost_joint_states = copy.deepcopy(atlasState_msg);
self.ghost_joint_states.position = list(self.ghost_joint_states.position)
#print "Initial Ghost Stored:",self.ghost_joint_states
# this runs in the Qt GUI thread and can therefore update the GUI
def on_updateGhost(self, ghost_joint_state_msg):
for i, new_joint_name in enumerate(list(ghost_joint_state_msg.name)):
name_found = False
for j,stored_joint_name in enumerate(self.ghost_joint_states.name):
if (new_joint_name == stored_joint_name):
name_found = True
#print "ghost_joint_msg[",str(i),"]=",ghost_joint_state_msg.position[i]
#print "ghost_joint_states[",str(j),"]=",self.ghost_joint_states.position[i]
self.ghost_joint_states.position[j] = ghost_joint_state_msg.position[i]
#self.ghost_joint_states.velocity[j] = ghost_joint_state_msg.velocity[i]
if (not name_found):
# Update the list without regard to order
self.ghost_joint_states.name.append(new_joint_name)
self.ghost_joint_states.position.append(ghost_joint_state_msg.position[i])
class PositionModeWidget(QObject):
updateStateSignal = Signal(object)
updatePelvisSignal = Signal(object)
updateGhostSignal = Signal(object)
def __init__(self, context):
#super(PositionModeDialog, self).__init__(context)
#self.setObjectName('PositionModeDialog')
super(PositionModeWidget, self).__init__()
self.name = 'PositionModeWidget'
self.updateStateSignal.connect(self.on_updateState)
self.updatePelvisSignal.connect(self.on_updatePelvis)
self.updateGhostSignal.connect(self.on_updateGhost)
## Process standalone plugin command-line arguments
#from argparse import ArgumentParser
#parser = ArgumentParser()
## Add argument(s) to the parser.
#parser.add_argument("-f", "--file",
# dest="file",
# help="Input file name")
#args, unknowns = parser.parse_known_args(context.argv())
#print 'arguments: ', args
#print 'unknowns: ', unknowns
#
#if ((args.file == "") or (args.file == None)):
# print "No file specified - abort!"
# sys.exit(-1)
# Define how long the trajectories will take to execute
self.time_factor = rospy.get_param('~time_factor', 2.1)
print "Using ",self.time_factor, " as the time factor for all trajectories"
# Joint Name : Child Link :OSRF
self.joint_names=[]
self.joint_names.append('back_bkz') # : ltorso : 0
self.joint_names.append('back_bky') # : mtorso : 1
self.joint_names.append('back_bkx') # : utorso : 2
self.joint_names.append('neck_ry') # : head : 3
self.joint_names.append('l_leg_hpz') # : l_uglut : 4
self.joint_names.append('l_leg_hpx') # : l_lglut : 5
self.joint_names.append('l_leg_hpy') # : l_uleg : 6
self.joint_names.append('l_leg_kny') # : l_lleg : 7
self.joint_names.append('l_leg_aky') # : l_talus : 8
self.joint_names.append('l_leg_akx') # : l_foot : 9
self.joint_names.append('r_leg_hpz') # : r_uglut : 10
self.joint_names.append('r_leg_hpx') # : r_lglut : 11
self.joint_names.append('r_leg_hpy') # : r_uleg : 12
self.joint_names.append('r_leg_kny') # : r_lleg : 13
self.joint_names.append('r_leg_aky') # : r_talus : 14
self.joint_names.append('r_leg_akx') # : r_foot : 15
self.joint_names.append('l_arm_shz') # : l_clav : 16
self.joint_names.append('l_arm_shx') # : l_scap : 17
self.joint_names.append('l_arm_ely') # : l_uarm : 18
self.joint_names.append('l_arm_elx') # : l_larm : 19
self.joint_names.append('l_arm_wry') # : l_farm : 20
self.joint_names.append('l_arm_wrx') # : l_hand : 21
self.joint_names.append('l_arm_wry2') # : l_farm : 22
self.joint_names.append('r_arm_shz') # : r_clav : 23
self.joint_names.append('r_arm_shx') # : r_scap : 24
self.joint_names.append('r_arm_ely') # : r_uarm : 25
self.joint_names.append('r_arm_elx') # : r_larm : 26
self.joint_names.append('r_arm_wry') # : r_farm : 27
self.joint_names.append('r_arm_wrx') # : r_hand : 28
self.joint_names.append('r_arm_wry2') # : r_farm : 29
self.joint_states = JointState()
self.pelvis_names=[]
self.joint_names.append('forward') # : 30
self.joint_names.append('lateral') # : 31
self.joint_names.append('height') # : 32
self.joint_names.append('roll') # : 33
self.joint_names.append('pitch') # : 34
self.joint_names.append('yaw') # : 35
self.pelvis_states = JointState()
self.pelvis_states.position = [0.0, 0.0, 0.85, 0.0, 0.0, 0.0] # default pelvis pose
#self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget();
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_ghost_target,0)
self.radioGroup.addButton(self.radio_robot_target,1)
self.radio_ghost_target.setChecked(True)
self.commandGroup = QButtonGroup()
self.commandGroup.setExclusive(True)
self.radio_position_command = QRadioButton()
self.radio_position_command.setText("Position")
self.commandGroup.addButton(self.radio_position_command,0)
self.radio_trajectory_command = QRadioButton()
self.radio_trajectory_command.setText("Trajectory")
self.commandGroup.addButton(self.radio_trajectory_command,1)
self.radio_trajectory_command.setChecked(True)
#print "position button is checked: ",self.radio_position_command.isChecked()
#print "trajectory button is checked: ",self.radio_trajectory_command.isChecked()
print "Default group button checked is ",self.radioGroup.checkedId()
print "Default command button checked is ",self.commandGroup.checkedId()
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
#hbox_radio.addWidget(QLabel("Ghost"))
hbox_radio.addStretch()
vbox_robot = QVBoxLayout()
widget_robot = QWidget()
widget_robot_sel = QWidget()
hbox_sel = QHBoxLayout()
hbox_radio.addStretch()
hbox_sel.addWidget(self.radio_robot_target)
#hbox_sel.addWidget(QLabel("Robot"))
hbox_radio.addStretch()
widget_robot_sel.setLayout(hbox_sel)
vbox_robot.addWidget(widget_robot_sel);
widget_cmd = QWidget()
hbox_cmd = QHBoxLayout()
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_position_command)
#hbox_cmd.addWidget(QLabel("Position"))
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_trajectory_command)
#hbox_cmd.addWidget(QLabel("Trajectory"))
hbox_radio.addStretch()
widget_cmd.setLayout(hbox_cmd)
vbox_robot.addWidget(widget_cmd);
widget_robot.setLayout(vbox_robot)
hbox_radio.addWidget(widget_robot)
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
positions_widget = QWidget()
hbox = QHBoxLayout()
#self.robot = URDF.from_parameter_server()
self.chains=[]
# Subscribe to Joint States update /atlas/atlas_state
#print "Load parameters"
#print rospy.get_param_names()
#fileName = rospy.get_param('positions_file', '/opt/vigir/catkin_ws/src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch/r_arm_positions.txt')
#print "FileName:",fileName
# Left to right layout
numFiles = rospy.get_param("~num_files");
for i in range(1,numFiles+1):
path = "~position_files/file_" + str(i)
foobar = str(rospy.get_param(path))
self.chains.append(ChainData(self, foobar, hbox))
#add stretch at end so all GUI elements are at left of dialog
hbox.addStretch(1)
positions_widget.setLayout(hbox)
vbox.addWidget(positions_widget)
vbox.addStretch(1)
self._widget.setLayout(vbox)
#context.add_widget(self._widget)
self.stateSubscriber = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
self.pelvisSubscriber = rospy.Subscriber('/robot_controllers/pelvis_traj_controller/current_states',JointState, self.pelvisCallbackFnc)
def shutdown_plugin(self):
#Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
#TODO, is the above comment why the plugin gets stuck when we try to shutdown?
print "Shutdown"
rospy.sleep(0.1)
self.stateSubscriber.unregister()
self.ghostSubscriber.unregister()
self.pelvisSubscriber.unregister()
def stateCallbackFnc(self, jointState_msg):
self.updateStateSignal.emit(jointState_msg)
def on_updateState(self, jointState_msg):
self.joint_states = jointState_msg
def pelvisCallbackFnc(self, jointState_msg):
self.updatePelvisSignal.emit(jointState_msg)
def on_updatePelvis(self, jointState_msg):
self.pelvis_states = jointState_msg;
def ghostCallbackFnc(self, jointState_msg):
self.updateGhostSignal.emit(jointState_msg)
def on_updateGhost(self, ghost_joint_state_msg):
self.ghost_joint_states = ghost_joint_state_msg
class JointControlWidget(QObject):
updateStateSignal = Signal(object)
updateGhostSignal = Signal(object)
def __init__(self, context):
super(JointControlWidget, self).__init__()
self.updateStateSignal.connect(self.on_update_state)
self.updateGhostSignal.connect(self.on_update_ghost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget()
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radioGroup.addButton(self.radio_ghost_target, 0)
self.radio_ghost_target.setChecked(True)
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_robot_target, 1)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
duration_box = QHBoxLayout()
duration_box.setAlignment(Qt.AlignLeft)
duration_box.addWidget(QLabel("Trajectory duration (s):"))
self.traj_duration_spin = QDoubleSpinBox()
self.traj_duration_spin.setValue(1.0)
self.traj_duration_spin.valueChanged.connect(self.on_traj_duration_changed)
duration_box.addWidget(self.traj_duration_spin)
self.update_controllers_buttonn = QPushButton("Update Controllers")
self.update_controllers_buttonn.pressed.connect(self.on_update_controllers)
duration_box.addWidget(self.update_controllers_buttonn)
vbox.addLayout(duration_box)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = JointControl(self, hbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
print "Add buttons to apply all ..."
all_widget = QWidget()
all_box = QHBoxLayout()
self.snap_to_ghost_button = QPushButton("SnapAllGhost")
self.snap_to_ghost_button.pressed.connect(self.on_snap_ghost_pressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snap_current_pressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_apply_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
self.apply_to_robot_button = QPushButton("Apply WBC Robot")
self.apply_to_robot_button.pressed.connect(self.on_apply_wbc_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
override_box = QHBoxLayout()
self.override = QCheckBox()
self.override.setChecked(False)
self.override.stateChanged.connect(self.on_override_changed)
override_box.addWidget(self.override)
override_label = QLabel("SAFETY OVERRIDE")
override_label.setStyleSheet('QLabel { color: red }')
override_box.addWidget(override_label)
override_box.addStretch()
vbox.addLayout(override_box)
vbox.addStretch()
self._widget.setLayout(vbox)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/joint_states', JointState, self.state_callback_fnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghost_callback_fnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states', JointState, queue_size=10)
self.time_last_update_state = time.time()
self.time_last_update_ghost = time.time()
def shutdown_plugin(self):
# Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
print "Shutdown ..."
self.stateSubscriber.unregister()
self.ghostSubscriber.unregister()
self.wbc_robot_pub.unregister()
print "Done!"
def state_callback_fnc(self, atlas_state_msg):
self.updateStateSignal.emit(atlas_state_msg)
def command_callback_fnc(self, atlas_command_msg):
self.updateCommandSignal.emit(atlas_command_msg)
def ghost_callback_fnc(self, ghost_joint_state_msg):
self.updateGhostSignal.emit(ghost_joint_state_msg)
def on_update_controllers(self):
self.joint_control.update_controllers()
def on_override_changed(self):
if self.override.isChecked():
print "WARNING: TURNING OFF SAFETY"
for controller in self.joint_control.controllers:
controller.set_override(True)
else:
print "Enabling safety checks"
for controller in self.joint_control.controllers:
controller.set_override(False)
def on_snap_ghost_pressed(self):
print "Snap all joint values to current ghost joint positions"
for controller in self.joint_control.controllers:
controller.on_snap_ghost_pressed()
def on_snap_current_pressed(self):
print "Snap all joint values to current joint positions"
for controller in self.joint_control.controllers:
controller.on_snap_current_pressed()
def on_apply_robot_pressed(self):
print "Send all latest joint values directly to robot"
for controller in self.joint_control.controllers:
controller.on_apply_robot_pressed()
def on_traj_duration_changed(self, duration):
self.joint_control.update_traj_duration(duration)
def on_apply_wbc_robot_pressed(self):
print "Send all latest joint values directly to robot as a WBC command"
if self.first_time:
print "Uninitialized !"
else:
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = copy.deepcopy(self.joint_states.name)
joint_state.position = list(copy.deepcopy(self.joint_states.position))
joint_state.velocity = list(copy.deepcopy(self.joint_states.velocity))
# print "Positions: ",joint_state.position
for i, name in enumerate(joint_state.name):
# for each joint, retrieve the data from each controller to populate the WBC vectors
for controller in self.joint_control.controllers:
for joint in controller.joints:
if joint.name == self.joint_states.name[i]:
print " fetching value for " + name + " = " + str(joint.position) + " at ndx=" + str(i)
joint_state.position[i] = joint.position
joint_state.velocity[i] = joint.velocity
# joint_state.effort[i] = joint.effort
self.wbc_robot_pub.publish(joint_state)
def on_update_state(self, atlas_state_msg):
if time.time() - self.time_last_update_state >= 0.2:
self.joint_states = atlas_state_msg
self.joint_control.update_joint_positions(self.joint_states)
if self.first_time:
self.joint_control.reset_current_joint_sliders()
self.first_time = False
self.ghost_joint_states = copy.deepcopy(atlas_state_msg)
self.ghost_joint_states.position = list(self.ghost_joint_states.position)
self.time_last_update_state = time.time()
# this runs in the Qt GUI thread and can therefore update the GUI
def on_update_ghost(self, ghost_joint_state_msg):
if time.time() - self.time_last_update_ghost >= 0.2:
for i, new_joint_name in enumerate(list(ghost_joint_state_msg.name)):
name_found = False
for j, stored_joint_name in enumerate(self.ghost_joint_states.name):
if new_joint_name == stored_joint_name:
name_found = True
self.ghost_joint_states.position[j] = ghost_joint_state_msg.position[i]
if not name_found:
# Update the list without regard to order
self.ghost_joint_states.name.append(new_joint_name)
self.ghost_joint_states.position.append(ghost_joint_state_msg.position[i])
self.time_last_update_ghost = time.time()