def generate_q_double_spin_box(default_val, range_min, range_max, decimals, single_step):
spin_box = QDoubleSpinBox()
spin_box.setValue(default_val)
spin_box.setRange(range_min, range_max)
spin_box.setDecimals(decimals)
spin_box.setSingleStep(single_step)
#spin_box.valueChanged[unicode].connect(self.callback_spin_box)
return spin_box
def generate_q_double_spin_box(default_val, range_min, range_max, decimals,
single_step):
spin_box = QDoubleSpinBox()
spin_box.setValue(default_val)
spin_box.setRange(range_min, range_max)
spin_box.setDecimals(decimals)
spin_box.setSingleStep(single_step)
#spin_box.valueChanged[unicode].connect(self.callback_spin_box)
return spin_box
class Joint(object):
def __init__(self, name, sensor_bias, control_bias, gearing_bias, controller):
self.name = name
self.sensor_bias = sensor_bias
self.control_bias = control_bias
self.controller = controller
self.gearing_bias = gearing_bias
print "Initial data for joint ",self.name, " biases=(",self.sensor_bias,", ",self.control_bias,")"
self.max_sensor_bias = 3.5
self.min_sensor_bias = -3.5
self.range_sensor_bias = self.max_sensor_bias - self.min_sensor_bias
self.max_control_bias = 0.3
self.min_control_bias = -0.3
self.max_gearing_bias = 1.3
self.min_gearing_bias = 0.7
self.range_gearing_bias = self.max_gearing_bias - self.min_gearing_bias
self.range_control_bias = self.max_control_bias - self.min_control_bias
self.sensor_bias_spinbox = QDoubleSpinBox()
self.sensor_bias_spinbox.setDecimals(4);
self.sensor_bias_spinbox.setRange(self.min_sensor_bias, self.max_sensor_bias)
self.sensor_bias_spinbox.setValue(self.sensor_bias)
self.sensor_bias_spinbox.setSingleStep(self.range_sensor_bias/500)
self.sensor_bias_spinbox.valueChanged.connect(self.on_biasChanged)
self.control_bias_spinbox = QDoubleSpinBox()
self.control_bias_spinbox.setDecimals(4);
self.control_bias_spinbox.setRange(self.min_control_bias, self.max_control_bias)
self.control_bias_spinbox.setSingleStep(self.range_control_bias/50)
self.control_bias_spinbox.setValue(self.control_bias)
self.control_bias_spinbox.valueChanged.connect(self.on_biasChanged)
self.gearing_bias_spinbox = QDoubleSpinBox()
self.gearing_bias_spinbox.setDecimals(4);
self.gearing_bias_spinbox.setRange(self.min_gearing_bias, self.max_gearing_bias)
self.gearing_bias_spinbox.setSingleStep(self.range_gearing_bias/50)
self.gearing_bias_spinbox.setValue(self.gearing_bias)
self.gearing_bias_spinbox.valueChanged.connect(self.on_biasChanged)
def on_biasChanged(self, value):
# grab both if either change
self.sensor_bias = self.sensor_bias_spinbox.value()
self.control_bias = self.control_bias_spinbox.value()
self.gearing_bias = self.gearing_bias_spinbox.value()
joint_state = JointState()
joint_state.name = [self.name];
joint_state.position = [self.sensor_bias];
joint_state.effort = [self.control_bias];
joint_state.velocity = [self.gearing_bias];
print "Set biases:\n ",joint_state
self.controller.parent.parent.bias_pub.publish(joint_state)
def saveData(self, saveFilePtr):
saveFilePtr.write(","+self.name+"/"+str(self.sensor_bias)+"/"+str(self.control_bias)+"/"+str(self.gearing_bias))
def createEditor(self, parent, option, index):
editor = QDoubleSpinBox(parent)
editor.setDecimals(self._decimals)
editor.setMaximum(self._min)
editor.setMaximum(self._max)
return editor
class FootstepParamControlWidget(QObject):
def __init__(self, context):
#super(FootstepParamControlDialog, self).__init__(context)
#self.setObjectName('FootstepParamControlDialog')
super(FootstepParamControlWidget, self).__init__()
self.name = 'FootstepParamControlWidget'
#self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
### STEP_COST_ESTIMATOR ########
self.step_cost_vbox = QVBoxLayout()
self.step_cost_groupbox = QGroupBox( "Step Cost Estimator" )
self.step_cost_groupbox.setCheckable( True )
self.step_cost_groupbox.setChecked(False)
self.step_cost_combobox = QComboBox()
self.step_cost_combobox.addItem( "Euclidean" )
self.step_cost_combobox.addItem( "GPR" )
self.step_cost_combobox.addItem( "Map" )
self.step_cost_combobox.addItem( "Boundary" )
self.step_cost_combobox.addItem( "Dynamics" )
self.step_cost_vbox.addWidget( self.step_cost_combobox )
self.step_cost_groupbox.setLayout( self.step_cost_vbox )
vbox.addWidget( self.step_cost_groupbox )
# #HARD ### FOOTSTEP_SET ########
# # parameters for discret footstep planning mode
# vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set # set of footsteps (displacement vectors (in meter / rad))
# float32[] footstep_cost # cost for each footstep given in footstep set
#
# #HARD ### LOAD_GPR_STEP_COST ########
# # map step cost file
# std_msgs/String map_step_cost_file
#
# #HARD ### LOAD_MAP_STEP_COST ########
# # destination of gpr file
# std_msgs/String gpr_step_cost_file
### COLLISION_CHECK_TYPE ########
self.collision_check_groupbox = QGroupBox( "Collision Check Type" )
self.collision_check_groupbox.setCheckable( True )
self.collision_check_groupbox.setChecked( False)
self.collision_check_vbox = QVBoxLayout()
self.collision_check_feet_checkbox = QCheckBox( "Feet" )
self.collision_check_vbox.addWidget( self.collision_check_feet_checkbox )
self.collision_check_ub_checkbox = QCheckBox( "Upper Body" )
self.collision_check_vbox.addWidget( self.collision_check_ub_checkbox )
self.collision_check_fc_checkbox = QCheckBox( "Foot Contact Support" )
self.collision_check_vbox.addWidget( self.collision_check_fc_checkbox )
self.collision_check_groupbox.setLayout( self.collision_check_vbox )
vbox.addWidget( self.collision_check_groupbox )
### FOOT_SIZE ########
self.foot_size_vbox = QVBoxLayout()
self.foot_size_groupbox = QGroupBox( "Foot Size" )
self.foot_size_groupbox.setCheckable( True )
self.foot_size_groupbox.setChecked( False )
self.foot_size_hbox = QHBoxLayout()
self.foot_size_label = QLabel("Foot Size")
self.foot_size_hbox.addWidget( self.foot_size_label )
self.foot_size_x = QDoubleSpinBox()
self.foot_size_x.setSingleStep(.01)
self.foot_size_hbox.addWidget(self.foot_size_x)
self.foot_size_y = QDoubleSpinBox()
self.foot_size_y.setSingleStep(.01)
self.foot_size_hbox.addWidget(self.foot_size_y)
self.foot_size_z = QDoubleSpinBox()
self.foot_size_z.setSingleStep(.01)
self.foot_size_hbox.addWidget(self.foot_size_z)
self.foot_size_vbox.addLayout( self.foot_size_hbox )
self.foot_shift_hbox = QHBoxLayout()
self.foot_shift_label = QLabel("Foot Origin Shift")
self.foot_shift_hbox.addWidget( self.foot_shift_label )
self.foot_shift_x = QDoubleSpinBox()
self.foot_shift_x.setRange(-1.0, 1.0)
self.foot_shift_x.setSingleStep(.01)
self.foot_shift_hbox.addWidget(self.foot_shift_x)
self.foot_shift_y = QDoubleSpinBox()
self.foot_shift_y.setRange(-1.0, 1.0)
self.foot_shift_y.setSingleStep(.01)
self.foot_shift_hbox.addWidget(self.foot_shift_y)
self.foot_shift_z = QDoubleSpinBox()
self.foot_shift_z.setRange(-1.0, 1.0)
self.foot_shift_z.setSingleStep(.01)
self.foot_shift_hbox.addWidget(self.foot_shift_z)
self.foot_size_vbox.addLayout( self.foot_shift_hbox )
self.foot_size_groupbox.setLayout( self.foot_size_vbox )
vbox.addWidget( self.foot_size_groupbox )
### UPPER_BODY_SIZE ########
self.upper_vbox = QVBoxLayout()
self.upper_groupbox = QGroupBox( "Upper Body Size" )
self.upper_groupbox.setCheckable( True )
self.upper_groupbox.setChecked( False )
self.upper_size_hbox = QHBoxLayout()
self.upper_size_label = QLabel("Upper Body Size")
self.upper_size_hbox.addWidget( self.upper_size_label )
self.upper_size_x = QDoubleSpinBox()
self.upper_size_x.setSingleStep(.01)
self.upper_size_hbox.addWidget(self.upper_size_x)
self.upper_size_y = QDoubleSpinBox()
self.upper_size_y.setSingleStep(.01)
self.upper_size_hbox.addWidget(self.upper_size_y)
self.upper_size_z = QDoubleSpinBox()
self.upper_size_z.setSingleStep(.01)
self.upper_size_hbox.addWidget(self.upper_size_z)
self.upper_vbox.addLayout( self.upper_size_hbox )
self.upper_origin_hbox = QHBoxLayout()
self.upper_origin_label = QLabel("Upper Body Origin")
self.upper_origin_hbox.addWidget( self.upper_origin_label )
self.upper_origin_x = QDoubleSpinBox()
self.upper_origin_x.setRange(-1.0, 1.0)
self.upper_origin_x.setSingleStep(.01)
self.upper_origin_hbox.addWidget(self.upper_origin_x)
self.upper_origin_y = QDoubleSpinBox()
self.upper_origin_y.setRange(-1.0, 1.0)
self.upper_origin_y.setSingleStep(.01)
self.upper_origin_hbox.addWidget(self.upper_origin_y)
self.upper_origin_z = QDoubleSpinBox()
self.upper_origin_z.setRange(-1.0, 1.0)
self.upper_origin_z.setSingleStep(.01)
self.upper_origin_hbox.addWidget(self.upper_origin_z)
self.upper_vbox.addLayout( self.upper_origin_hbox )
self.upper_groupbox.setLayout( self.upper_vbox )
vbox.addWidget( self.upper_groupbox )
### TERRAIN_MODEL ########
self.terrain_model_groupbox = QGroupBox( "Terrain Model" )
self.terrain_model_groupbox.setCheckable( True )
self.terrain_model_groupbox.setChecked( False )
self.terrain_model_vbox = QVBoxLayout()
self.terrain_model_checkbox = QCheckBox( "Use Terrain Model" )
self.terrain_model_vbox.addWidget( self.terrain_model_checkbox )
self.terrain_min_ssx_hbox = QHBoxLayout()
self.terrain_min_ssx_label = QLabel("Min Sampling Steps x")
self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_label )
self.terrain_min_ssx_val = QDoubleSpinBox()
self.terrain_min_ssx_val.setSingleStep(1)
self.terrain_min_ssx_val.setRange(0,100)
self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_val )
self.terrain_model_vbox.addLayout( self.terrain_min_ssx_hbox )
self.terrain_min_ssy_hbox = QHBoxLayout()
self.terrain_min_ssy_label = QLabel("Min Sampling Steps y")
self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_label )
self.terrain_min_ssy_val = QDoubleSpinBox()
self.terrain_min_ssy_val.setSingleStep(1)
self.terrain_min_ssy_val.setRange(0,100)
self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_val )
self.terrain_model_vbox.addLayout( self.terrain_min_ssy_hbox )
self.terrain_max_ssx_hbox = QHBoxLayout()
self.terrain_max_ssx_label = QLabel("Max Sampling Steps x")
self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_label )
self.terrain_max_ssx_val = QDoubleSpinBox()
self.terrain_max_ssx_val.setSingleStep(1)
self.terrain_max_ssx_val.setRange(0,100)
self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_val )
self.terrain_model_vbox.addLayout( self.terrain_max_ssx_hbox )
self.terrain_max_ssy_hbox = QHBoxLayout()
self.terrain_max_ssy_label = QLabel("Max Sampling Steps y")
self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_label )
self.terrain_max_ssy_val = QDoubleSpinBox()
self.terrain_max_ssy_val.setSingleStep(1)
self.terrain_max_ssy_val.setRange(0,100)
self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_val )
self.terrain_model_vbox.addLayout( self.terrain_max_ssy_hbox )
self.terrain_max_iz_hbox = QHBoxLayout()
self.terrain_max_iz_label = QLabel("Max Intrusion z")
self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_label )
self.terrain_max_iz_val = QDoubleSpinBox()
self.terrain_max_iz_val.setDecimals(4)
self.terrain_max_iz_val.setSingleStep(.0001)
self.terrain_max_iz_val.setRange(0,.25)
self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_val )
self.terrain_model_vbox.addLayout( self.terrain_max_iz_hbox )
self.terrain_max_gc_hbox = QHBoxLayout()
self.terrain_max_gc_label = QLabel("Max Ground Clearance")
self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_label )
self.terrain_max_gc_val = QDoubleSpinBox()
self.terrain_max_gc_val.setDecimals(4)
self.terrain_max_gc_val.setSingleStep(.0001)
self.terrain_max_gc_val.setRange(0,.25)
self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_val )
self.terrain_model_vbox.addLayout( self.terrain_max_gc_hbox )
self.terrain_ms_hbox = QHBoxLayout()
self.terrain_ms_label = QLabel("Minimal Support")
self.terrain_ms_hbox.addWidget( self.terrain_ms_label )
self.terrain_ms_val = QDoubleSpinBox()
self.terrain_ms_val.setDecimals(2)
self.terrain_ms_val.setSingleStep(.01)
self.terrain_ms_val.setRange(0,1)
self.terrain_ms_hbox.addWidget( self.terrain_ms_val )
self.terrain_model_vbox.addLayout( self.terrain_ms_hbox )
self.terrain_model_groupbox.setLayout( self.terrain_model_vbox )
vbox.addWidget( self.terrain_model_groupbox )
### STANDARD_STEP_PARAMS ########
self.std_step_vbox = QVBoxLayout()
self.std_step_groupbox = QGroupBox( "Standard Step Parameters" )
self.std_step_groupbox.setCheckable( True )
self.std_step_groupbox.setChecked( False )
self.foot_sep_hbox = QHBoxLayout()
self.foot_sep_label = QLabel("Foot Separation")
self.foot_sep_hbox.addWidget( self.foot_sep_label )
self.foot_sep_val = QDoubleSpinBox()
self.foot_sep_val.setSingleStep(.01)
self.foot_sep_hbox.addWidget(self.foot_sep_val)
self.std_step_vbox.addLayout( self.foot_sep_hbox )
self.std_step_step_duration_hbox = QHBoxLayout()
self.std_step_step_duration_label = QLabel("Step Duration")
self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_label )
self.std_step_step_duration_val = QDoubleSpinBox()
self.std_step_step_duration_val.setSingleStep(.01)
self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_val )
self.std_step_vbox.addLayout( self.std_step_step_duration_hbox )
self.std_step_sway_duration_hbox = QHBoxLayout()
self.std_step_sway_duration_label = QLabel("Sway Duration")
self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_label )
self.std_step_sway_duration_val = QDoubleSpinBox()
self.std_step_sway_duration_val.setSingleStep(.01)
self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_val )
self.std_step_vbox.addLayout( self.std_step_sway_duration_hbox )
self.std_step_swing_height_hbox = QHBoxLayout()
self.std_step_swing_height_label = QLabel("Swing Height")
self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_label )
self.std_step_swing_height_val = QDoubleSpinBox()
self.std_step_swing_height_val.setSingleStep(.01)
self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_val )
self.std_step_vbox.addLayout( self.std_step_swing_height_hbox )
self.std_step_lift_height_hbox = QHBoxLayout()
self.std_step_lift_height_label = QLabel("Lift Height")
self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_label )
self.std_step_lift_height_val = QDoubleSpinBox()
self.std_step_lift_height_val.setSingleStep(.01)
self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_val )
self.std_step_vbox.addLayout( self.std_step_lift_height_hbox )
self.std_step_groupbox.setLayout( self.std_step_vbox )
vbox.addWidget( self.std_step_groupbox )
button_hbox = QHBoxLayout()
button_get = QPushButton("Get Current Values")
button_hbox.addWidget( button_get )
button_submit = QPushButton("Send Values")
button_hbox.addWidget( button_submit)
vbox.addLayout( button_hbox )
vbox.addStretch(1)
self._widget.setLayout(vbox)
#context.add_widget(self._widget)
# publishers and subscribers
self.param_pub = rospy.Publisher('/flor/footstep_planner/set_params', FootstepPlannerParams, queue_size=10)
button_submit.pressed.connect(self.sendParams)
self.param_sub = self.stateSubscriber = rospy.Subscriber('/ros_footstep_planner/params', FootstepPlannerParams, self.getParamCallbackFcn)
button_get.pressed.connect(self.getParams)
def shutdown_plugin(self):
print "Shutdown ..."
self.param_pub.unregister()
print "Done!"
def getParams( self ):
msg = FootstepPlannerParams()
msg.change_mask = 0
self.param_pub.publish( msg )
def sendParams( self ):
msg = FootstepPlannerParams()
msg.change_mask = 0
if self.step_cost_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x001
if self.collision_check_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x010
if self.foot_size_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x020
if self.upper_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x040
if self.std_step_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x080
if self.terrain_model_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x100
### STEP_COST_ESTIMATOR ########
msg.step_cost_type = self.step_cost_combobox.currentIndex()
# #HARD ### FOOTSTEP_SET ########
# # parameters for discret footstep planning mode
# vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set # set of footsteps (displacement vectors (in meter / rad))
# float32[] footstep_cost # cost for each footstep given in footstep set
#
# #HARD ### LOAD_GPR_STEP_COST ########
# # map step cost file
# std_msgs/String map_step_cost_file
#
# #HARD ### LOAD_MAP_STEP_COST ########
# # destination of gpr file
# std_msgs/String gpr_step_cost_file
### COLLISION_CHECK_TYPE ########
msg.collision_check_type = 0
if self.collision_check_feet_checkbox.isChecked():
msg.collision_check_type = msg.collision_check_type | 0x01
if self.collision_check_ub_checkbox.isChecked():
msg.collision_check_type = msg.collision_check_type | 0x02
if self.collision_check_fc_checkbox.isChecked():
msg.collision_check_type = msg.collision_check_type | 0x04
### FOOT_SIZE ########
msg.foot_size.x = self.foot_size_x.value()
msg.foot_size.y = self.foot_size_y.value()
msg.foot_size.z = self.foot_size_z.value()
msg.foot_origin_shift.x = self.foot_shift_x.value()
msg.foot_origin_shift.y = self.foot_shift_y.value()
msg.foot_origin_shift.z = self.foot_shift_z.value()
### UPPER_BODY_SIZE ########
msg.upper_body_size.x = self.upper_size_x.value()
msg.upper_body_size.y = self.upper_size_y.value()
msg.upper_body_size.z = self.upper_size_z.value()
msg.upper_body_origin_shift.x = self.upper_origin_x.value()
msg.upper_body_origin_shift.y = self.upper_origin_y.value()
msg.upper_body_origin_shift.z = self.upper_origin_z.value()
### TERRAIN_MODEL ########
msg.use_terrain_model = self.terrain_model_checkbox.isChecked()
msg.min_sampling_steps_x = self.terrain_min_ssx_val.value()
msg.min_sampling_steps_y = self.terrain_min_ssy_val.value()
msg.max_sampling_steps_x = self.terrain_max_ssx_val.value()
msg.max_sampling_steps_y = self.terrain_max_ssy_val.value()
msg.max_intrusion_z = self.terrain_max_iz_val.value()
msg.max_ground_clearance = self.terrain_max_gc_val.value()
msg.minimal_support = self.terrain_ms_val.value()
### STANDARD_STEP_PARAMS ########
msg.foot_seperation = self.foot_sep_val.value()
msg.step_duration = self.std_step_step_duration_val.value()
msg.sway_duration = self.std_step_sway_duration_val.value()
msg.swing_height = self.std_step_swing_height_val.value()
msg.lift_height = self.std_step_lift_height_val.value()
print msg
self.param_pub.publish( msg )
def getParamCallbackFcn(self, msg):
### STEP_COST_ESTIMATOR ########
if msg.change_mask & 0x001:
self.step_cost_combobox.setCurrentIndex( msg.step_cost_type )
# #HARD ### FOOTSTEP_SET ########
# # parameters for discret footstep planning mode
# vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set # set of footsteps (displacement vectors (in meter / rad))
# float32[] footstep_cost # cost for each footstep given in footstep set
#
# #HARD ### LOAD_GPR_STEP_COST ########
# # map step cost file
# std_msgs/String map_step_cost_file
#
# #HARD ### LOAD_MAP_STEP_COST ########
# # destination of gpr file
# std_msgs/String gpr_step_cost_file
### COLLISION_CHECK_TYPE ########
if msg.change_mask & 0x010:
self.collision_check_feet_checkbox.setChecked( msg.collision_check_type & 0x01 )
self.collision_check_ub_checkbox.setChecked( msg.collision_check_type & 0x02 )
self.collision_check_fc_checkbox.setChecked( msg.collision_check_type & 0x04 )
### FOOT_SIZE ########
if msg.change_mask & 0x020:
self.foot_size_x.setValue( msg.foot_size.x )
self.foot_size_y.setValue( msg.foot_size.y )
self.foot_size_z.setValue( msg.foot_size.z )
self.foot_shift_x.setValue( msg.foot_origin_shift.x )
self.foot_shift_y.setValue( msg.foot_origin_shift.y )
self.foot_shift_z.setValue( msg.foot_origin_shift.z )
### UPPER_BODY_SIZE ########
if msg.change_mask & 0x040:
self.upper_size_x.setValue( msg.upper_body_size.x )
self.upper_size_y.setValue( msg.upper_body_size.y )
self.upper_size_z.setValue( msg.upper_body_size.z )
self.upper_origin_x.setValue( msg.upper_body_origin_shift.x )
self.upper_origin_y.setValue( msg.upper_body_origin_shift.y )
self.upper_origin_z.setValue( msg.upper_body_origin_shift.z )
### STANDARD_STEP_PARAMS ########
if msg.change_mask & 0x080:
self.foot_sep_val.setValue( msg.foot_seperation )
self.std_step_step_duration_val.setValue( msg.step_duration )
self.std_step_sway_duration_val.setValue( msg.sway_duration )
self.std_step_swing_height_val.setValue( msg.swing_height )
self.std_step_lift_height_val.setValue( msg.lift_height )
### TERRAIN_MODEL ########
if msg.change_mask & 0x100:
self.terrain_model_checkbox.setChecked( msg.use_terrain_model )
self.terrain_min_ssx_val.setValue( msg.min_sampling_steps_x )
self.terrain_min_ssy_val.setValue( msg.min_sampling_steps_y )
self.terrain_max_ssx_val.setValue( msg.max_sampling_steps_x )
self.terrain_max_ssy_val.setValue( msg.max_sampling_steps_y )
self.terrain_max_iz_val.setValue( msg.max_intrusion_z )
self.terrain_max_gc_val.setValue( msg.max_ground_clearance )
self.terrain_ms_val.setValue( msg.minimal_support )
class FootstepTerrainControlDialog(Plugin):
def __init__(self, context):
super(FootstepTerrainControlDialog, self).__init__(context)
self.setObjectName('FootstepTerrainControlDialog')
self.request_seq = 0
self._widget = QWidget()
vbox = QVBoxLayout()
self.cube_vbox = QVBoxLayout()
self.cube_groupbox = QGroupBox( "Terrain Box" )
self.cube_min_hbox = QHBoxLayout()
self.cube_min_label = QLabel("Bottom Left")
self.cube_min_hbox.addWidget( self.cube_min_label )
self.cube_min_x = QDoubleSpinBox()
self.cube_min_x.setSingleStep(.01)
self.cube_min_x.setRange(-10.0, 10.0)
self.cube_min_x.setValue(-3.0)
self.cube_min_hbox.addWidget(self.cube_min_x)
self.cube_min_y = QDoubleSpinBox()
self.cube_min_y.setSingleStep(.01)
self.cube_min_y.setRange(-10.0, 10.0)
self.cube_min_y.setValue(-3.0)
self.cube_min_hbox.addWidget(self.cube_min_y)
self.cube_min_z = QDoubleSpinBox()
self.cube_min_z.setSingleStep(.01)
self.cube_min_z.setRange(-10.0, 10.0)
self.cube_min_z.setValue(-1.0)
self.cube_min_hbox.addWidget(self.cube_min_z)
self.cube_vbox.addLayout( self.cube_min_hbox )
self.cube_max_hbox = QHBoxLayout()
self.cube_max_label = QLabel("Top Right")
self.cube_max_hbox.addWidget( self.cube_max_label )
self.cube_max_x = QDoubleSpinBox()
self.cube_max_x.setSingleStep(.01)
self.cube_max_x.setRange(-10.0, 10.0)
self.cube_max_x.setValue(3.0)
self.cube_max_hbox.addWidget(self.cube_max_x)
self.cube_max_y = QDoubleSpinBox()
self.cube_max_y.setSingleStep(.01)
self.cube_max_y.setRange(-10.0, 10.0)
self.cube_max_y.setValue(3.0)
self.cube_max_hbox.addWidget(self.cube_max_y)
self.cube_max_z = QDoubleSpinBox()
self.cube_max_z.setSingleStep(.01)
self.cube_max_z.setRange(-10.0, 10.0)
self.cube_max_z.setValue(1.0)
self.cube_max_hbox.addWidget(self.cube_max_z)
self.cube_vbox.addLayout( self.cube_max_hbox )
self.cube_groupbox.setLayout( self.cube_vbox )
vbox.addWidget( self.cube_groupbox )
self.type_hbox = QHBoxLayout()
# self.type_label = QLabel( "Type" )
# self.type_hbox.addWidget( self.type_label )
#
# self.type_combobox = QComboBox()
#
# self.type_combobox.addItem( "Type 1" )
# self.type_combobox.addItem( "Type 2" )
# self.type_combobox.addItem( "Type 3" )
#
# self.type_hbox.addWidget( self.type_combobox )
#
# vbox.addLayout( self.type_hbox )
self.scan_number_hbox = QHBoxLayout()
self.scan_number_label = QLabel("Number of Scans Used")
self.scan_number_hbox.addWidget( self.scan_number_label )
self.scan_number_val = QDoubleSpinBox()
self.scan_number_val.setDecimals(0)
self.scan_number_val.setRange(0,10000)
self.scan_number_val.setValue(2000)
self.scan_number_hbox.addWidget(self.scan_number_val)
vbox.addLayout( self.scan_number_hbox )
# self.use_terrain_checkbox = QCheckBox( "Use Terrain" )
# vbox.addWidget( self.use_terrain_checkbox )
button_hbox = QHBoxLayout()
# button_get = QPushButton("Get Current Values")
# button_hbox.addWidget( button_get )
button_submit = QPushButton("Send Values")
button_hbox.addWidget( button_submit)
vbox.addLayout( button_hbox )
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
# publishers and subscribers
self.terrain_pub = rospy.Publisher("/flor/terrain_classifier/generate_terrain_model", TerrainModelRequest, queue_size=10 )
button_submit.pressed.connect(self.sendParams)
# self.terrain_sub = self.stateSubscriber = rospy.Subscriber('/ros_footstep_planner/terrains', FootstepPlannerParams, self.getParamCallbackFcn)
# button_submit.pressed.connect(self.getParams)
def shutdown_plugin(self):
print "Shutdown ..."
print "Done!"
# def getParams( self ):
# msg = FootstepPlannerParams()
#
#
# self.terrain_pub.publish( msg )
def sendParams( self ):
msg = TerrainModelRequest()
msg.use_default_region_request = False
rr = EnvironmentRegionRequest()
rr.header.seq = self.request_seq
self.request_seq = self.request_seq+1
rr.header.stamp = rospy.get_rostime()
rr.header.frame_id = "/world"
rr.bounding_box_min.x = self.cube_min_x.value()
rr.bounding_box_min.y = self.cube_min_y.value()
rr.bounding_box_min.z = self.cube_min_z.value()
rr.bounding_box_max.x = self.cube_max_x.value()
rr.bounding_box_max.y = self.cube_max_y.value()
rr.bounding_box_max.z = self.cube_max_z.value()
rr.resolution = 0
rr.request_augment = 0
msg.region_req = rr
msg.aggregation_size = self.scan_number_val.value()
print msg
self.terrain_pub.publish( msg )
def getParamCallbackFcn(self, msg):
print "Not Defined Yet"
def createEditor(self, parent, option, index):
editor = QDoubleSpinBox(parent)
editor.setDecimals(self._decimals)
editor.setMaximum(self._min)
editor.setMaximum(self._max)
return editor
class SpeakEasyGUI(QMainWindow):
'''
One instance of this class builds the entire sound play UI.
Instance variable that hold widgets of interest to controllers:
- C{speechInputFld}
- C{onceOrRepeatDict}
- C{voicesRadioButtonsDict}
- C{recorderDict}
- C{programButtonDict}
- C{soundButtonDict}
'''
# ---------------------- Durations --------------------
# Number of seconds a program button must be pressed to
# go into program mode:
PROGRAM_BUTTON_HOLD_TIME = 3.0;
# Amount of time the program button stays in alternate look
# to indicate that it is changing to programming mode:
PROGRAM_BUTTON_LOOK_CHANGE_DURATION = 0.2; # seconds
# ---------------------- Button and Font Sizes --------------------
# Minimum height of buttons:
BUTTON_MIN_HEIGHT = 30; # pixels
# Pushbutton minimum font size:
BUTTON_LABEL_FONT_SIZE = 16; # pixels
# Radio button minimum font size:
RADIO_BUTTON_LABEL_FONT_SIZE = 16; # pixels
EDIT_FIELD_TEXT_SIZE = 18; # pixels
NUM_OF_PROGRAM_BUTTON_COLUMNS = 4;
NUM_OF_SOUND_BUTTON_COLUMNS = 4;
# ---------------------- Names for Voices ----------------------
# Official names of voices as recognized by the
# underlying text-to-speech engines:
voices = {'VOICE_1': 'voice_kal_diphone', # Festival voice
'VOICE_2': 'David', # Cepstral voices
'VOICE_3': 'Amy',
'VOICE_4': 'Shouty',
'VOICE_5': 'Whispery',
'VOICE_6': 'Lawrence',
'VOICE_7': 'William'
};
# ---------------------- Names for Widgets ----------------------
interactionWidgets = {
'PLAY_ONCE': 'Play once',
'PLAY_REPEATEDLY': 'Play repeatedly',
'PLAY_REPEATEDLY_PERIOD': 'Pause between plays',
'VOICE_1': 'Machine',
'VOICE_2': 'David',
'VOICE_3': 'Amy',
'VOICE_4': 'Shout',
'VOICE_5': 'Whisper',
'VOICE_6': 'Lawrence',
'VOICE_7': 'William',
'PLAY_TEXT': 'Play Text',
'STOP': 'Stop',
'STOP_ALL' : 'Stop All',
'SPEECH_1' : 'Speech 1',
'SPEECH_2' : 'Speech 2',
'SPEECH_3' : 'Speech 3',
'SPEECH_4' : 'Speech 4',
'SPEECH_5' : 'Speech 5',
'SPEECH_6' : 'Speech 6',
'SPEECH_7' : 'Speech 7',
'SPEECH_8' : 'Speech 8',
'SPEECH_9' : 'Speech 9',
'SPEECH_10' : 'Speech 10',
'SPEECH_11' : 'Speech 11',
'SPEECH_12' : 'Speech 12',
'SPEECH_13' : 'Speech 13',
'SPEECH_14' : 'Speech 14',
'SPEECH_15' : 'Speech 15',
'SPEECH_16' : 'Speech 16',
'SPEECH_17' : 'Speech 17',
'SPEECH_18' : 'Speech 18',
'SPEECH_19' : 'Speech 19',
'SPEECH_20' : 'Speech 20',
'SPEECH_21' : 'Speech 21',
'SPEECH_22' : 'Speech 22',
'SPEECH_23' : 'Speech 23',
'SPEECH_24' : 'Speech 24',
'SOUND_1' : 'Rooster',
'SOUND_2' : 'Drill',
'SOUND_3' : 'Bull call',
'SOUND_4' : 'Clown horn',
'SOUND_5' : 'Cash register',
'SOUND_6' : 'Glass breaking',
'SOUND_7' : 'Cell door',
'SOUND_8' : 'Cow',
'SOUND_9' : 'Birds',
'SOUND_10' : 'Big truck',
'SOUND_11' : 'Seagulls',
'SOUND_12' : 'Lift',
'NEW_SPEECH_SET' : 'Save speech set',
'PICK_SPEECH_SET' : 'Pick different speech set',
'PLAY_LOCALLY' : 'Play locally',
'PLAY_AT_ROBOT' : 'Play at robot',
'SPEECH_MODULATION' : 'Speech modulation',
'PASTE' : 'Paste',
'CLEAR' : 'Clear',
}
# ---------------------- Application Background styling --------------------
# Application background:
veryLightBlue = QColor(230, 255,255);
stylesheetAppBG = 'QDialog {background-color: %s}' % veryLightBlue.name();
defaultStylesheet = stylesheetAppBG;
defaultStylesheetName = "Default";
# ---------------------- Edit field styling -----------------
editFieldBGColor = QColor(12,21,109);
editFieldTextColor = QColor(244,244,246);
inputFldStylesheet =\
'TextPanel {background-color: ' + editFieldBGColor.name() +\
'; color: ' + editFieldTextColor.name() +\
'; font-size: ' + str(EDIT_FIELD_TEXT_SIZE) + 'pt' +\
'}';
# ---------------------- Pushbutton styling --------------------
# Button color definitions:
recorderButtonBGColor = QColor(176,220,245); # Light blue
recorderButtonDisabledBGColor = QColor(187,200,208); # Gray-blue
recorderButtonTextColor = QColor(0,0,0); # Black
programButtonBGColor = QColor(117,150,169); # Middle blue
programButtonTextColor = QColor(251,247,247);# Off-white
soundButtonBGColor = QColor(110,134,211); # Dark blue
soundButtonTextColor = QColor(251,247,247); # Off-white
# Button stylesheets:
recorderButtonStylesheet =\
'QPushButton {background-color: ' + recorderButtonBGColor.name() +\
'; color: ' + recorderButtonTextColor.name() +\
'; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
'}';
recorderButtonDisabledStylesheet =\
'QPushButton {background-color: ' + recorderButtonDisabledBGColor.name() +\
'; color: ' + recorderButtonTextColor.name() +\
'; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
'}';
programButtonStylesheet =\
'QPushButton {background-color: ' + programButtonBGColor.name() +\
'; color: ' + programButtonTextColor.name() +\
'; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
'}';
# Stylesheet for when program button look is temporarily
# changed to indicate transition to programming mode:
programButtonModeTransitionStylesheet =\
'QPushButton {background-color: ' + editFieldBGColor.name() +\
'; color: ' + programButtonTextColor.name() +\
'; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
'}';
soundButtonStylesheet =\
'QPushButton {background-color: ' + soundButtonBGColor.name() +\
'; color: ' + soundButtonTextColor.name() +\
'; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
'}';
# ---------------------- Radiobutton and Play Repeat Delay Spinbox styling -----------------
# Radiobutton color definitions:
playOnceRepeatButtonBGColor = QColor(121,229,230); # Very light cyan
voicesButtonBGColor = QColor(97,164,165); # Darker cyan
playOnceRepeatButtonStylesheet =\
'font-size: ' + str(RADIO_BUTTON_LABEL_FONT_SIZE) + 'px' +\
'; color: ' + soundButtonTextColor.name() +\
'; background-color: ' + voicesButtonBGColor.name();
playRepeatSpinboxStylesheet =\
'font-size: ' + str(RADIO_BUTTON_LABEL_FONT_SIZE) + 'px' +\
'; color: ' + soundButtonTextColor.name() +\
'; background-color: ' + voicesButtonBGColor.name();
# voiceButtonStylesheet =\
# 'font-size: ' + str(RADIO_BUTTON_LABEL_FONT_SIZE) + 'px' +\
# '; background-color: ' + voicesButtonBGColor.name();
voiceButtonStylesheet =\
'font-size: ' + str(RADIO_BUTTON_LABEL_FONT_SIZE) + 'px' +\
'; color: ' + soundButtonTextColor.name() +\
'; background-color: ' + voicesButtonBGColor.name();
# ---------------------- Signals -----------------
hideButtonSignal = pyqtSignal(QPushButton); # hide the given button
showButtonSignal = pyqtSignal(QPushButton); # show the hidden button
#----------------------------------
# Initializer
#--------------
def __init__(self, parent=None, mirrored=True, stand_alone=False, sound_effect_labels=None):
super(SpeakEasyGUI, self).__init__(parent);
self.stand_alone = stand_alone;
self.sound_effect_labels = sound_effect_labels;
if (sound_effect_labels is None):
raise ValueError("Must pass in non-null array of sound effect button labels.");
#self.setMaximumWidth(1360);
#self.setMaximumHeight(760);
# Vertical box to hold all top level widget groups
appLayout = QVBoxLayout();
appWidget = QDialog();
appWidget.setStyleSheet(SpeakEasyGUI.defaultStylesheet);
# Activate the window resize handle in the lower right corner
# of the app window:
appWidget.setSizeGripEnabled(True);
self.setCentralWidget(appWidget);
self.addTitle(appLayout);
self.addTxtInputFld(appLayout);
self.buildTapeRecorderButtons(appLayout);
self.addOnceOrRepeat_And_VoiceRadioButtons(appLayout);
self.buildHorizontalDivider(appLayout);
self.buildProgramButtons(appLayout);
self.buildSoundButtons(appLayout);
self.buildButtonSetControls(appLayout);
self.buildOptionsRadioButtons(appLayout);
appWidget.setLayout(appLayout);
#*******self.connectEvents();
self.show();
#----------------------------------
# programButtonIterator
#--------------
def programButtonIterator(self, gridLayout=None):
if gridLayout is None:
gridLayout = self.programButtonGridLayout;
programButtonArray = [];
# Collect the buttons into a flat array:
for row in range(gridLayout.rowCount()):
for col in range(SpeakEasyGUI.NUM_OF_PROGRAM_BUTTON_COLUMNS):
layoutItem = gridLayout.itemAtPosition(row, col);
if layoutItem is not None:
programButtonArray.append(layoutItem.widget());
return iter(programButtonArray);
#----------------------------------
# replaceProgramButtons
#--------------
def replaceProgramButtons(self, buttonProgramsArray):
programButtonObjIt = self.programButtonIterator();
# Remove the existing program buttons from the application's
# layout, and mark them for deletion:
try:
while True:
programButtonObj = programButtonObjIt.next();
self.programButtonGridLayout.removeWidget(programButtonObj);
programButtonObj.deleteLater();
except StopIteration:
pass
# Make an array of button labels from the ButtonProgram
# instances in buttonProgramsArray:
buttonLabelArr = [];
for buttonProgram in buttonProgramsArray:
buttonLabelArr.append(buttonProgram.getLabel());
# No more program buttons present. Make
# new ones, and add them to the grid layout:
(newProgramButtonGridLayout, self.programButtonDict) =\
SpeakEasyGUI.buildButtonGrid(buttonLabelArr,
SpeakEasyGUI.NUM_OF_PROGRAM_BUTTON_COLUMNS);
for buttonObj in self.programButtonDict.values():
buttonObj.setStyleSheet(SpeakEasyGUI.programButtonStylesheet);
buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
# Transfer the buttons from this new gridlayout object to the
# original layout that is embedded in the application Dialog:
newButtonsIt = self.programButtonIterator(gridLayout=newProgramButtonGridLayout);
try:
while True:
self.programButtonGridLayout.addWidget(newButtonsIt.next());
except StopIteration:
pass;
#----------------------------------
# connectEvents
#--------------
def connectEvents(self):
mousePressEvent.connect(self.textAreaMouseClick);
def textAreaMouseClick(self, event):
print('Text area clicked.')
#----------------------------------
# connectSignalsToWidgets
#--------------
# def connectSignalsToWidgets(self):
# self.commChannel = CommChannel();
# self.commChannel.hideButtonSignal.connect(SpeakEasyGUI.hideButtonHandler);
# self.commChannel.showButtonSignal.connect(SpeakEasyGUI.showButtonHandler);
#----------------------------------
# addTitle
#--------------
def addTitle(self,layout):
title = QLabel("<H1>SpeakEasy</H1>");
hbox = QHBoxLayout();
hbox.addStretch(1);
hbox.addWidget(title);
hbox.addStretch(1);
layout.addLayout(hbox);
#----------------------------------
# addTxtInputFld
#--------------
def addTxtInputFld(self, layout):
'''
Creates text input field label and text field
in a horizontal box layout. Adds that hbox layout
to the passed-in layout.
Sets instance variables:
1. C{self.speechInputFld}
@param layout: Layout object to which the label/txt-field C{hbox} is to be added.
@type layout: QLayout
'''
speechControlsLayout = QHBoxLayout();
speechControlsLayout.addStretch(1);
speechInputFldLabel = QLabel("<b>What to say:</b>")
speechControlsLayout.addWidget(speechInputFldLabel);
self.speechInputFld = TextPanel(numLines=5);
self.speechInputFld.setStyleSheet(SpeakEasyGUI.inputFldStylesheet);
self.speechInputFld.setFontPointSize(SpeakEasyGUI.EDIT_FIELD_TEXT_SIZE);
speechControlsLayout.addWidget(self.speechInputFld);
layout.addLayout(speechControlsLayout);
# Create and hide the dialog for adding Cepstral voice modulation
# markup to text in the text input field:
self.speechControls = MarkupManagementUI(textPanel=self.speechInputFld, parent=self);
#----------------------------------
# addVoiceRadioButtons
#--------------
def addOnceOrRepeat_And_VoiceRadioButtons(self, layout):
'''
Creates radio buttons for selecting whether a
sound is to play once, or repeatedly until stopped.
Also adds radio buttons for selecting voices.
Places all in a horizontal box layout. Adds
that hbox layout to the passed-in layout.
Sets instance variables:
1. C{self.onceOrRepeatDict}
2. C{self.voicesRadioButtonsDict}
@param layout: Layout object to which the label/txt-field C{hbox} is to be added.
@type layout: QLayout
'''
hbox = QHBoxLayout();
(self.onceOrRepeatGroup, onceOrRepeatButtonLayout, self.onceOrRepeatDict) =\
self.buildRadioButtons([SpeakEasyGUI.interactionWidgets['PLAY_ONCE'],
SpeakEasyGUI.interactionWidgets['PLAY_REPEATEDLY']
],
Orientation.HORIZONTAL,
Alignment.LEFT,
activeButtons=[SpeakEasyGUI.interactionWidgets['PLAY_ONCE']],
behavior=CheckboxGroupBehavior.RADIO_BUTTONS);
self.replayPeriodSpinBox = QDoubleSpinBox(self);
self.replayPeriodSpinBox.setRange(0.0, 99.9); # seconds
self.replayPeriodSpinBox.setSingleStep(0.5);
self.replayPeriodSpinBox.setDecimals(1);
onceOrRepeatButtonLayout.addWidget(self.replayPeriodSpinBox);
secondsLabel = QLabel("secs delay");
onceOrRepeatButtonLayout.addWidget(secondsLabel);
# Create an array of voice radio button labels:
voiceRadioButtonLabels = [];
for voiceKey in SpeakEasyGUI.voices.keys():
voiceRadioButtonLabels.append(SpeakEasyGUI.interactionWidgets[voiceKey]);
(self.voicesGroup, voicesButtonLayout, self.voicesRadioButtonsDict) =\
self.buildRadioButtons(voiceRadioButtonLabels,
Orientation.HORIZONTAL,
Alignment.RIGHT,
activeButtons=[SpeakEasyGUI.interactionWidgets['VOICE_1']],
behavior=CheckboxGroupBehavior.RADIO_BUTTONS);
# Style all the radio buttons:
for playFreqButton in self.onceOrRepeatDict.values():
playFreqButton.setStyleSheet(SpeakEasyGUI.playOnceRepeatButtonStylesheet);
for playFreqButton in self.voicesRadioButtonsDict.values():
playFreqButton.setStyleSheet(SpeakEasyGUI.voiceButtonStylesheet);
#...and the replay delay spinbox:
self.replayPeriodSpinBox.setStyleSheet(SpeakEasyGUI.playRepeatSpinboxStylesheet);
#****** replayPeriodSpinBox styling
hbox.addLayout(onceOrRepeatButtonLayout);
hbox.addStretch(1);
hbox.addLayout(voicesButtonLayout);
layout.addLayout(hbox);
#----------------------------------
# buildTapeRecorderButtons
#--------------
def buildTapeRecorderButtons(self, layout):
'''
Creates tape recorder buttons (Play Text, Stop,etc.).
Places all in a row, though the layout is a
QGridLayout. Adds QGridLayout to the passed-in
layout.
Sets instance variables:
1. C{self.recorderDict}
@param layout: Layout object to which the label/txt-field C{QGridlayout} is to be added.
@type layout: QLayout
'''
(buttonGridLayout, self.recorderButtonDict) =\
SpeakEasyGUI.buildButtonGrid([SpeakEasyGUI.interactionWidgets['PLAY_TEXT'],
SpeakEasyGUI.interactionWidgets['STOP'],
#SpeakEasyGUI.interactionWidgets['STOP_ALL'] # Stop button already stops all.
],
2); # Two columns
for buttonObj in self.recorderButtonDict.values():
buttonObj.setStyleSheet(SpeakEasyGUI.recorderButtonStylesheet);
buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
layout.addLayout(buttonGridLayout);
#----------------------------------
# buildProgramButtons
#--------------
def buildProgramButtons(self, layout):
'''
Creates grid of buttons for saving sounds.
Adds the resulting QGridLayout to the passed-in
layout.
Sets instance variables:
1. C{self.programButtonDict}
@param layout: Layout object to which the label/txt-field C{QGridlayout} is to be added.
@type layout: QLayout
'''
buttonLabelArr = [];
for i in range(NUM_SPEECH_PROGRAM_BUTTONS):
key = "SPEECH_" + str(i+1);
buttonLabelArr.append(SpeakEasyGUI.interactionWidgets[key]);
(self.programButtonGridLayout, self.programButtonDict) =\
SpeakEasyGUI.buildButtonGrid(buttonLabelArr,
SpeakEasyGUI.NUM_OF_PROGRAM_BUTTON_COLUMNS);
for buttonObj in self.programButtonDict.values():
buttonObj.setStyleSheet(SpeakEasyGUI.programButtonStylesheet);
buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
layout.addLayout(self.programButtonGridLayout);
#----------------------------------
# buildSoundButtons
#--------------
def buildSoundButtons(self, layout):
'''
Creates grid of buttons for playing canned sounds.
Adds the resulting QGridLayout to the passed-in
layout.
Sets instance variables:
1. C{self.soundButtonDict}
@param layout: Layout object to which the label/txt-field C{QGridlayout} is to be added.
@type layout: QLayout
'''
for i in range(len(self.sound_effect_labels)):
key = "SOUND_" + str(i);
SpeakEasyGUI.interactionWidgets[key] = self.sound_effect_labels[i];
(buttonGridLayout, self.soundButtonDict) =\
SpeakEasyGUI.buildButtonGrid(self.sound_effect_labels, SpeakEasyGUI.NUM_OF_SOUND_BUTTON_COLUMNS);
for buttonObj in self.soundButtonDict.values():
buttonObj.setStyleSheet(SpeakEasyGUI.soundButtonStylesheet);
buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
layout.addLayout(buttonGridLayout);
#----------------------------------
# buildButtonSetControls
#--------------
def buildButtonSetControls(self, layout):
buttonLabelArray = [self.interactionWidgets['NEW_SPEECH_SET'], self.interactionWidgets['PICK_SPEECH_SET']];
# Two columns of buttons:
(buttonGridLayout, self.speechSetButtonDict) = SpeakEasyGUI.buildButtonGrid(buttonLabelArray, 2);
for buttonObj in self.speechSetButtonDict.values():
buttonObj.setStyleSheet(SpeakEasyGUI.programButtonStylesheet);
buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
layout.addLayout(buttonGridLayout);
#----------------------------------
# buildOptionsRadioButtons
#--------------
def buildOptionsRadioButtons(self, layout):
hbox = QHBoxLayout();
(self.playLocalityGroup, playLocalityButtonLayout, self.playLocalityRadioButtonsDict) =\
self.buildRadioButtons([SpeakEasyGUI.interactionWidgets['PLAY_LOCALLY'],
SpeakEasyGUI.interactionWidgets['PLAY_AT_ROBOT']
],
Orientation.HORIZONTAL,
Alignment.LEFT,
activeButtons=[SpeakEasyGUI.interactionWidgets[DEFAULT_PLAY_LOCATION]],
behavior=CheckboxGroupBehavior.RADIO_BUTTONS);
#behavior=CheckboxGroupBehavior.CHECKBOXES);
# Style all the radio buttons:
for playLocalityButton in self.playLocalityRadioButtonsDict.values():
playLocalityButton.setStyleSheet(SpeakEasyGUI.voiceButtonStylesheet);
hbox.addLayout(playLocalityButtonLayout);
hbox.addStretch(1);
self.buildConvenienceButtons(hbox);
layout.addLayout(hbox);
#----------------------------------
# buildConvenienceButtons
#--------------
def buildConvenienceButtons(self, layout):
'''
Creates buttons meant for accessibility convenience.
Example: Paste.
Places all in a row, though the layout is a
QGridLayout. Adds QGridLayout to the passed-in
layout.
Sets instance variables:
1. C{self.convenienceButtonDict}
@param layout: Layout object to which the label/txt-field C{QGridlayout} is to be added.
@type layout: QLayout
'''
(buttonGridLayout, self.convenienceButtonDict) =\
SpeakEasyGUI.buildButtonGrid([SpeakEasyGUI.interactionWidgets['SPEECH_MODULATION'],
SpeakEasyGUI.interactionWidgets['PASTE'],
SpeakEasyGUI.interactionWidgets['CLEAR'],
],
2); # Two columns
for buttonObj in self.convenienceButtonDict.values():
buttonObj.setStyleSheet(SpeakEasyGUI.recorderButtonStylesheet);
buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
layout.addLayout(buttonGridLayout);
#----------------------------------
# buildHorizontalDivider
#--------------
def buildHorizontalDivider(self, layout):
frame = QFrame();
frame.setFrameShape(QFrame.HLine);
#*******frame.setFrameStyle(QFrame.Shadow.Sunken.value());
frame.setLineWidth(3);
frame.setMidLineWidth(3);
layout.addWidget(frame);
#----------------------------------
# buildRadioButtons
#--------------
# Note: Whenever a button is switched on or off it emits the
# PySide.QtGui.QAbstractButton.toggled() signal. Connect to this
# signal if you want to trigger an action each time the button changes state.
def buildRadioButtons(self,
labelTextArray,
orientation,
alignment,
activeButtons=None,
behavior=CheckboxGroupBehavior.RADIO_BUTTONS):
'''
@param labelTextArray: Names of buttons
@type labelTextArray: [string]
@param orientation: Whether to arrange the buttons vertically or horizontally.
@type orientation: Orientation
@param alignment: whether buttons should be aligned Left/Center/Right for horizontal,
Top/Center/Bottom for vertical:/c
@type alignment: Alignment
@param activeButtons: Name of the buttons that is to be checked initially. Or None.
@type activeButtons: [string]
@param behavior: Indicates whether the button group is to behave like Radio Buttons, or like Checkboxes.
@type behavior: CheckboxGroupBehavior
@return
1. The button group that contains the related buttons. Caller: ensure that
this object does not go out of scope.
2. The button layout, which callers will need to add to
their own layouts.
3. and a dictionary mapping button names to button objects that
were created within this method. This dict is needed by the
controller.
@rtype (QButtonGroup, QLayout, dict<string,QRadioButton>).
'''
# Button control management group. It has no visible
# representation. It allows the radio button bahavior, for instance:
buttonGroup = QButtonGroup();
if behavior == CheckboxGroupBehavior.CHECKBOXES:
buttonGroup.setExclusive(False);
else:
buttonGroup.setExclusive(True);
if orientation == Orientation.VERTICAL:
buttonCompLayout = QVBoxLayout();
# Arrange for the alignment (Part 1):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.BOTTOM):
buttonCompLayout.addStretch(1);
else:
buttonCompLayout = QHBoxLayout();
# Arrange for the alignment (Part 1):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.RIGHT):
buttonCompLayout.addStretch(1);
# Build the buttons:
buttonDict = {};
for label in labelTextArray:
button = QRadioButton(label);
buttonDict[label] = button;
# Add button to the button management group:
buttonGroup.addButton(button);
# Add the button to the visual group:
buttonCompLayout.addWidget(button);
if label in activeButtons:
button.setChecked(True);
if orientation == Orientation.VERTICAL:
buttonCompLayout = QVBoxLayout();
# Arrange for the alignment (Part 2):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.TOP):
buttonCompLayout.addStretch(1);
else:
# Arrange for the alignment (Part 2):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.LEFT):
buttonCompLayout.addStretch(1);
return (buttonGroup, buttonCompLayout, buttonDict);
#----------------------------------
# buildButtonGrid
#--------------
@staticmethod
def buildButtonGrid(labelTextArray,
numColumns):
'''
Creates a grid of QPushButton widgets. They will be
@param labelTextArray: Button labels.
@type labelTextArray: [string]
@param numColumns: The desired width of the button grid.
@type numColumns: int
@return: 1. a grid layout with the button objects inside.
2. a dictionary mapping button labels to button objects.
@rtype: QGridLayout
'''
buttonLayout = QGridLayout();
# Compute number of button rows:
numRows = len(labelTextArray) / numColumns;
# If this number of rows leaves a few buttons left over (< columnNum),
# then add a row for those:
if (len(labelTextArray) % numColumns) > 0:
numRows += 1;
buttonDict = {}
row = 0;
col = 0;
for buttonLabel in labelTextArray:
button = QPushButton(buttonLabel);
buttonDict[buttonLabel] = button;
buttonLayout.addWidget(button, row, col);
col += 1;
if (col > (numColumns - 1)):
col = 0;
row += 1;
return (buttonLayout, buttonDict);
#----------------------------------
# getNewButtonLabel
#--------------
def getNewButtonLabel(self):
'''
Requests a new button label from the user.
Returns None if user canceled out, or a string
with the new button label.
@return: None if user canceled, else string from input field.
@rtype: {None | string}
'''
prompt = "New label for this button:";
dialogBox = QInputDialog(parent=self);
dialogBox.setStyleSheet(SpeakEasyGUI.stylesheetAppBG);
dialogBox.setLabelText(prompt);
dialogBox.setCancelButtonText("Keep existing label");
userChoice = dialogBox.exec_();
if userChoice == QDialog.Accepted:
return dialogBox.textValue();
else:
return None;
#----------------------------------
# playOnceChecked
#--------------
def playOnceChecked(self):
return self.onceOrRepeatDict[SpeakEasyGUI.interactionWidgets['PLAY_ONCE']].isChecked();
#----------------------------------
# setPlayOnceChecked
#--------------
def setPlayOnceChecked(self):
self.onceOrRepeatDict[SpeakEasyGUI.interactionWidgets['PLAY_ONCE']].setChecked(True);
#----------------------------------
# playRepeatedlyChecked
#--------------
def playRepeatedlyChecked(self):
return self.onceOrRepeatDict[SpeakEasyGUI.interactionWidgets['PLAY_REPEATEDLY']].isChecked();
#----------------------------------
# setPlayRepeatedlyChecked
#--------------
def setPlayRepeatedlyChecked(self):
self.onceOrRepeatDict[SpeakEasyGUI.interactionWidgets['PLAY_REPEATEDLY']].setChecked(True);
#----------------------------------
# playRepeatPeriod
#--------------
def getPlayRepeatPeriod(self):
return self.replayPeriodSpinBox.value();
#----------------------------------
# activeVoice
#--------------
def activeVoice(self):
'''
Return the official name of the voice that is currently
checked in the UI. This is the name that will be recognized
by the underlying text-to-speech engine(s).
@return: Name of voice as per the SpeakEasyGUI.voices dict.
@rtype: string
'''
for voiceKey in SpeakEasyGUI.voices.keys():
if self.voicesRadioButtonsDict[SpeakEasyGUI.interactionWidgets[voiceKey]].isChecked():
return SpeakEasyGUI.voices[voiceKey];
#----------------------------------
# whereToPlay
#--------------
def whereToPlay(self):
'''
Returns which of the play location options is selected: Locally or Robot.
@return: Selection of where sound and text-to-speech output is to occur.
@rtype: PlayLocation
'''
if self.playLocalityRadioButtonsDict[SpeakEasyGUI.interactionWidgets['PLAY_LOCALLY']].isChecked():
return PlayLocation.LOCALLY;
else:
return PlayLocation.ROBOT;
#----------------------------------
# setWhereToPlay
#--------------
def setWhereToPlay(self, playLocation):
'''
Set the option radio button that determines where sound is produced,
locally, or at the robot. No action is taken. This method merely sets
the appropriate radio button.
@param playLocation: PlayLocation.LOCALLY, or PlayLocation.ROBOT
@type playLocation: PlayLocation
'''
if playLocation == PlayLocation.LOCALLY:
radioButton = self.playLocalityRadioButtonsDict[SpeakEasyGUI.interactionWidgets['PLAY_LOCALLY']];
else:
radioButton = self.playLocalityRadioButtonsDict[SpeakEasyGUI.interactionWidgets['PLAY_AT_ROBOT']];
radioButton.setChecked(True);
#----------------------------------
# setButtonLabel
#--------------
def setButtonLabel(self, buttonObj, label):
buttonObj.setText(label);
#----------------------------------
# blinkButton
#--------------
def blinkButton(self, buttonObj, turnOn):
'''
Used to make a program button blink in some way to indicate
that it is changing into programming mode. Since this method
is triggered by a timer thread, it cannot make any GUI changes.
Instead, it sends a signal to have the GUI thread place the
button into an alternative look. It then schedules a call
to itself for a short time later. At that point it sends a
signal to the GUI thread to return the button to its usual
look:
@param buttonObj: The button to be blinked
@type buttonObj: QPushButton
@param turnOn: Indicates whether to turn the button back into
its normal state, or whether to make it take on
its alternate look.
@type turnOn: bool
'''
if turnOn:
self.showButtonSignal.emit(buttonObj);
else:
self.hideButtonSignal.emit(buttonObj);
self.buttonBlinkTimer = Timer(SpeakEasyGUI.PROGRAM_BUTTON_LOOK_CHANGE_DURATION, partial(self.blinkButton, buttonObj, True));
self.buttonBlinkTimer.start();
class SinusoidalTrajectoryDialog(Plugin):
updateStateSignal = Signal(object)
def __init__(self, context):
super(SinusoidalTrajectoryDialog, self).__init__(context)
self.setObjectName('SinusoidalTrajectoryDialog')
#self.updateStateSignal = Signal(object)
self.updateStateSignal.connect(self.on_updateState)
self.robot = URDF.from_parameter_server()
self.joint_list = {}
for ndx,jnt in enumerate(self.robot.joints):
self.joint_list[jnt.name] = ndx
#print jnt.name, " ",self.joint_list[jnt.name]
self.chain=[]
self.chain_file = rospy.get_param("chain_file")
self.chain_name = rospy.get_param("chain_name")
yaml_file = self.chain_file+self.chain_name+"_chain.yaml"
print yaml_file
#define structures
self.robot_state = JointState()
self.robot_command = JointTrajectory()
stream = open(yaml_file, "r")
jointChain = yaml.load_all(stream)
print '\n\n'
for ndx, data in enumerate(jointChain):
print ndx," : ", data
self.delay_time = data["delay_time"]
self.amplitude = data["amplitude"]
self.frequency = data["frequency"]
self.frequency_limit = data["frequency_limit"]
self.iterations = data["iterations"]
self.joint_state_topic = data["joint_state_topic"]
self.trajectory_topic = data["trajectory_topic"]
joints = rospy.get_param(data["chain_param_name"])
for joint in joints:
print joint
self.robot_state.name.append(joint)
self.chain.append(JointData(self, joint) )
self.robot_command.joint_names = self.robot_state.name
stream.close()
self.robot_state.position = [0.0]*len(self.robot_state.name)
self.robot_state.velocity = [0.0]*len(self.robot_state.name)
self.robot_state.effort = [0.0]*len(self.robot_state.name)
self.robot_joint_state = JointState()
print "delay_time =",self.delay_time
print "amplitude =",self.amplitude
print "frequency =",self.frequency
print "iterations =",self.iterations
print "Robot State Structure",self.robot_state
print "Robot Command Structure",self.robot_command
# initialize structure to hold widget handles
self.cur_position_spinbox=[]
self.amplitude_spinbox=[]
self.frequency_spinbox=[]
self.iterations_spinbox=[]
self._widget = QWidget()
vbox = QVBoxLayout()
# Push buttons
hbox = QHBoxLayout()
snap_command = QPushButton("Snap Position")
snap_command.clicked.connect(self.snap_current_callback)
hbox.addWidget(snap_command)
check_limits = QPushButton("Check Limits Gains")
check_limits.clicked.connect(self.check_limits_callback)
hbox.addWidget(check_limits)
apply_command = QPushButton("Send Trajectory")
apply_command.clicked.connect(self.apply_command_callback)
hbox.addWidget(apply_command)
save_trajectory = QPushButton("Save Trajectory")
save_trajectory.clicked.connect(self.save_trajectory_callback)
hbox.addWidget(save_trajectory)
zero_ramp = QPushButton("Zero Values")
zero_ramp.clicked.connect(self.zero_values_callback)
hbox.addWidget(zero_ramp)
vbox.addLayout(hbox)
time_hbox = QHBoxLayout()
vbox_frequqency = QVBoxLayout()
vbox_frequqency.addWidget(QLabel("Frequency"))
self.frequency_spinbox = QDoubleSpinBox()
self.frequency_spinbox.setDecimals(5)
self.frequency_spinbox.setRange(0, self.frequency_limit)
self.frequency_spinbox.setSingleStep(0.05)
self.frequency_spinbox.valueChanged.connect(self.on_frequency_value)
self.frequency_spinbox.setValue(self.frequency)
vbox_frequqency.addWidget(self.frequency_spinbox)
time_hbox.addLayout(vbox_frequqency)
vbox_iterations = QVBoxLayout()
vbox_iterations.addWidget(QLabel("Iterations"))
self.iterations_spinbox = QDoubleSpinBox()
self.iterations_spinbox.setDecimals(5)
self.iterations_spinbox.setRange(0, 10)
self.iterations_spinbox.setSingleStep(1)
self.iterations_spinbox.valueChanged.connect(self.on_iterations_value)
self.iterations_spinbox.setValue(self.iterations)
vbox_iterations.addWidget(self.iterations_spinbox)
time_hbox.addLayout(vbox_iterations)
vbox.addLayout(time_hbox)
# Joints title
title_frame = QFrame()
title_frame.setFrameShape(QFrame.StyledPanel);
title_frame.setFrameShadow(QFrame.Raised);
title_hbox = QHBoxLayout()
title_hbox.addWidget(QLabel("Joints"))
title_hbox.addWidget(QLabel("Current Position"))
title_hbox.addWidget(QLabel("Amplitude"))
title_frame.setLayout(title_hbox)
vbox.addWidget(title_frame)
# Define the widgets for each joint
for i,joint in enumerate(self.chain):
#print i,",",joint
self.joint_widget( vbox, i)
#add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
# Define the connections to the outside world
self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
self.commandPublisher = rospy.Publisher(self.trajectory_topic , JointTrajectory, queue_size=10)
# Add the widget
context.add_widget(self._widget)
@Slot()
def snap_current_callback(self):
self.blockSignals(True)
print "Snapping positions to actual values"
for index, joint in enumerate(self.chain):
for index_state, name_state in enumerate(self.robot_joint_state.name):
if (name_state == joint.name):
joint.position = copy.deepcopy(self.robot_joint_state.position[index_state])
self.cur_position_spinbox[index].setValue(joint.position)
break
self.blockSignals(False)
@Slot()
def check_limits_callback(self):
self.blockSignals(True)
print "Check limits callback ..."
valid = True
for index, joint in enumerate(self.chain):
ramp_up = joint.position + joint.amplitude
ramp_down = joint.position - joint.amplitude
p = self.amplitude_spinbox[index].palette()
if (ramp_up > joint.upper_limit) or (ramp_up < joint.lower_limit):
print "Joint ",joint.name, " is beyond upper limit!"
valid=False
p.setColor(QPalette.Window, Qt.red) # <<<<<<<<<<<----------------- This isn't working as intended
else:
p.setColor(QPalette.Window, Qt.white)# <<<<<<<<<<<----------------- This isn't working as intended
#print joint.lower_limit," < ", ramp_up, " < ",joint.upper_limit
self.amplitude_spinbox[index].setPalette(p)
if (ramp_down > joint.upper_limit) or (ramp_down < joint.lower_limit):
print "Joint ",joint.name, " is beyond lower limit!"
valid=False
p.setColor(QPalette.Window, Qt.red) # <<<<<<<<<<<----------------- This isn't working as intended
else:
p.setColor(QPalette.Window, Qt.white)# <<<<<<<<<<<----------------- This isn't working as intended
#print joint.lower_limit," < ", ramp_down, " < ",joint.upper_limit
if (self.frequency > self.frequency_limit or self.frequency <= 0):
print "invalid frequency. must be between 0 and ", self.frequency_limit, ". value is: ", self.frequency
valid = False
if (not valid):
print "ERROR: Invalid input!"
else:
print " Valid values!"
self.blockSignals(False)
return valid
@Slot()
def zero_values_callback(self):
self.blockSignals(True)
print "Zero ramps callback ..."
for index, joint in enumerate(self.chain):
self.amplitude_spinbox[index].setValue(0.0)
self.blockSignals(False)
@Slot()
def apply_command_callback(self):
self.blockSignals(True)
print "Send trajectory"
if self.calculate_trajectory():
#print self.robot_command;
self.commandPublisher.publish(self.robot_command)
else:
print "Trajectory calculation failure - do not publish!"
self.blockSignals(False)
@Slot()
def save_trajectory_callback(self):
self.blockSignals(True)
print "Save gains"
# Save data to file that we can read in
# TODO - invalid file names
fileName = QFileDialog.getSaveFileName()
#if fileName[0] checks to ensure that the file dialog has not been canceled
if fileName[0]:
if self.calculate_trajectory():
print self.robot_command;
newFileptr = open(fileName[0], 'w')
# Suggested format for files to make it easy to combine different outputs
newFileptr.write('# Trajectory \n')
newFileptr.write(self.robot_command)
newFileptr.write('\n')
newFileptr.close()
else:
print "Trajectory calculation failure - do not save!"
else:
print "Save cancelled."
newFilePtr.close()
self.blockSignals(False)
#
@Slot()
def stateCallbackFnc(self, atlasState_msg):
# Handle the state message for actual positions
time = atlasState_msg.header.stamp.to_sec()
if ((time - self.prior_time) > 0.02):
# Only update at 50hz
# relay the ROS messages through a Qt Signal to switch to the GUI thread
self.updateStateSignal.emit(atlasState_msg)
self.prior_time = time
# this runs in the Qt GUI thread and can therefore update the GUI
def on_updateState(self, joint_state_msg):
#print joint_state_msg
self.robot_joint_state = joint_state_msg
def on_delay_time_value(self, value):
self.delay_time = copy.deepcopy(value)
def on_frequency_value(self, value):
self.frequency = copy.deepcopy(value)
def on_iterations_value(self, value):
self.iterations = copy.deepcopy(value)
def calculate_trajectory(self):
if not self.check_limits_callback():
print "Invalid limits for trajectory!"
return False
knot_points = 8*self.iterations
if (knot_points < 2):
print "Invalid trajectory - knot_points = ",knot_points
return False
#print "Knot points=",knot_points
self.robot_command.points = []
self.robot_command.points.append(JointTrajectoryPoint())
ndx = 0
self.robot_command.points[ndx].time_from_start = rospy.Duration(0.0)
for jnt, joint in enumerate(self.chain):
self.robot_command.points[ndx].positions.append(joint.position)
self.robot_command.points[ndx].velocities.append(0.0)
self.robot_command.points[ndx].accelerations.append(0.0)
ndx += 1
dt = np.pi / (4.0 * self.frequency)
time_offset = dt
print "dt = ", dt
while ndx < knot_points-1:
self.robot_command.points.append(JointTrajectoryPoint())
time_offset += dt
print " time = ", time_offset
self.robot_command.points[ndx].time_from_start = rospy.Duration(time_offset)
for jnt, joint in enumerate(self.chain):
self.robot_command.points[ndx].positions.append(joint.position + joint.amplitude * np.sin(self.frequency * time_offset))
self.robot_command.points[ndx].velocities.append(self.frequency * joint.amplitude * np.cos(self.frequency * time_offset))
self.robot_command.points[ndx].accelerations.append(-self.frequency * self.frequency * joint.amplitude * np.sin(self.frequency * time_offset))
ndx += 1
#end position
self.robot_command.points.append(JointTrajectoryPoint())
time_offset += dt
self.robot_command.points[ndx].time_from_start = rospy.Duration(time_offset)
for jnt, joint in enumerate(self.chain):
self.robot_command.points[ndx].positions.append(joint.position)
self.robot_command.points[ndx].velocities.append(0.0)
self.robot_command.points[ndx].accelerations.append(0.0)
print self.robot_command
print "Ndx=",ndx, " of ",knot_points, "=",len(self.robot_command.points)
if (ndx != len(self.robot_command.points)-1) or (len(self.robot_command.points) != knot_points):
print "Invalid number of knot points - ignore trajectory"
print self.robot_command
return False
self.robot_command.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
return True;
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.jointSubscriber.unregister()
self.commandPublisher.unregister()
rospy.sleep(0.5)
print "Done!"
# Define collection of widgets for joint group
def joint_widget( self, main_vbox, index):
joint = self.chain[index]
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel);
frame.setFrameShadow(QFrame.Raised);
hbox = QHBoxLayout()
#hbox.addWidget(frame)
self.prior_time = 0.0
robot_joint = self.robot.joints[self.joint_list[joint.name]]
joint.lower_limit = robot_joint.limit.lower
joint.upper_limit = robot_joint.limit.upper
amplitude_range = (robot_joint.limit.upper-robot_joint.limit.lower)
frequency_range = self.frequency_limit
iterations_range = 10000
print " ",joint.name, " limits(", joint.lower_limit,", ",joint.upper_limit,") range=",amplitude_range
self.cur_position_spinbox.append(QDoubleSpinBox())
self.cur_position_spinbox[index].setDecimals(5)
self.cur_position_spinbox[index].setRange(joint.lower_limit, joint.upper_limit)
self.cur_position_spinbox[index].setSingleStep((robot_joint.limit.upper-robot_joint.limit.lower)/50.0)
self.cur_position_spinbox[index].valueChanged.connect(joint.on_position_value)
self.amplitude_spinbox.append(QDoubleSpinBox())
self.amplitude_spinbox[index].setDecimals(5)
self.amplitude_spinbox[index].setRange(-amplitude_range, amplitude_range)
self.amplitude_spinbox[index].setSingleStep(amplitude_range/50.0)
self.amplitude_spinbox[index].valueChanged.connect(joint.on_amplitude_value)
self.amplitude_spinbox[index].setValue(joint.amplitude)
hbox.addWidget(QLabel(joint.name))
hbox.addWidget(self.cur_position_spinbox[index])
hbox.addWidget(self.amplitude_spinbox[index])
# Add horizontal layout to frame for this joint group
frame.setLayout(hbox)
# Add frame to main vertical layout
main_vbox.addWidget(frame)
class TrapezoidalTrajectoryDialog(Plugin):
updateStateSignal = Signal(object)
def __init__(self, context):
super(TrapezoidalTrajectoryDialog, self).__init__(context)
self.setObjectName("TrapezoidalTrajectoryDialog")
# self.updateStateSignal = Signal(object)
self.updateStateSignal.connect(self.on_updateState)
self.robot = URDF.from_parameter_server()
self.joint_list = {}
for ndx, jnt in enumerate(self.robot.joints):
self.joint_list[jnt.name] = ndx
self.chain = []
self.chain_file = rospy.get_param("~chain_file")
self.chain_name = rospy.get_param("~chain_name")
print
print "Define order of splines used to approximate trapezoid"
self.spline_order = rospy.get_param("~spline_order", 5) # 1 # 3 # 5 # linear, cubic, quintic
if (self.spline_order == 1) or (self.spline_order == 3) or (self.spline_order == 5):
print "Spline order=", self.spline_order
else:
print "Invalid spline order! Must be 1, 3, or 5"
print "Spline order=", self.spline_order
sys.exit(-1)
yaml_file = self.chain_file + self.chain_name + "_chain.yaml"
print "Chain definition file:"
print yaml_file
print
# define structures
self.robot_state = JointState()
self.robot_command = JointTrajectory()
stream = open(yaml_file, "r")
jointChain = yaml.load_all(stream)
for ndx, data in enumerate(jointChain):
print ndx, " : ", data
self.delay_time = data["delay_time"]
self.ramp_up_time = data["ramp_up_time"]
self.dwell_time = data["dwell_time"]
self.ramp_down_time = data["ramp_down_time"]
self.hold_time = data["hold_time"]
self.ramp_start_fraction = data["ramp_start_fraction"]
self.ramp_end_fraction = data["ramp_end_fraction"]
self.joint_state_topic = data["joint_state_topic"]
self.trajectory_topic = data["trajectory_topic"]
if rospy.search_param(data["chain_param_name"]):
print "Found ", data["chain_param_name"]
else:
print "Failed to find the ", data["chain_param_name"], " in the parameter server!"
sys.exit(-1)
joint_names = rospy.get_param(data["chain_param_name"])
for joint in joint_names:
print joint
self.robot_state.name.append(joint)
self.chain.append(JointData(self, joint))
self.robot_command.joint_names = self.robot_state.name
stream.close()
self.robot_state.position = [0.0] * len(self.robot_state.name)
self.robot_state.velocity = [0.0] * len(self.robot_state.name)
self.robot_state.effort = [0.0] * len(self.robot_state.name)
self.robot_joint_state = JointState()
print "delay_time =", self.delay_time
print "ramp_up_time =", self.ramp_up_time
print "dwell_time =", self.dwell_time
print "ramp_down_time=", self.ramp_down_time
print "hold_time =", self.hold_time
print "spline order =", self.spline_order
if (
(self.ramp_start_fraction < 0.001)
or (self.ramp_end_fraction > 0.999)
or (self.ramp_start_fraction >= self.ramp_end_fraction)
):
print "Invalid ramp fractions - abort!"
print "0.0 < ", self.ramp_start_fraction, " < ", self.ramp_end_fraction, " < 1.0"
return
print "Robot State Structure", self.robot_state
print "Robot Command Structure", self.robot_command
# initialize structure to hold widget handles
self.cmd_spinbox = []
self.ramp_up_spinbox = []
self.ramp_down_spinbox = []
self._widget = QWidget()
vbox = QVBoxLayout()
# Push buttons
hbox = QHBoxLayout()
snap_command = QPushButton("Snap Position")
snap_command.clicked.connect(self.snap_current_callback)
hbox.addWidget(snap_command)
check_limits = QPushButton("Check Limits Gains")
check_limits.clicked.connect(self.check_limits_callback)
hbox.addWidget(check_limits)
apply_command = QPushButton("Send Trajectory")
apply_command.clicked.connect(self.apply_command_callback)
hbox.addWidget(apply_command)
save_trajectory = QPushButton("Save Trajectory")
save_trajectory.clicked.connect(self.save_trajectory_callback)
hbox.addWidget(save_trajectory)
zero_ramp = QPushButton("Zero Ramps")
zero_ramp.clicked.connect(self.zero_ramp_callback)
hbox.addWidget(zero_ramp)
vbox.addLayout(hbox)
time_hbox = QHBoxLayout()
vbox_delay = QVBoxLayout()
vbox_delay.addWidget(QLabel("Delay"))
self.delay_time_spinbox = QDoubleSpinBox()
self.delay_time_spinbox.setDecimals(5)
self.delay_time_spinbox.setRange(0, 10.0)
self.delay_time_spinbox.setSingleStep(0.1)
self.delay_time_spinbox.valueChanged.connect(self.on_delay_time_value)
self.delay_time_spinbox.setValue(self.delay_time)
vbox_delay.addWidget(self.delay_time_spinbox)
time_hbox.addLayout(vbox_delay)
vbox_ramp_up = QVBoxLayout()
vbox_ramp_up.addWidget(QLabel("Ramp Up"))
self.ramp_up_time_spinbox = QDoubleSpinBox()
self.ramp_up_time_spinbox.setDecimals(5)
self.ramp_up_time_spinbox.setRange(0, 10.0)
self.ramp_up_time_spinbox.setSingleStep(0.1)
self.ramp_up_time_spinbox.valueChanged.connect(self.on_ramp_up_time_value)
self.ramp_up_time_spinbox.setValue(self.ramp_up_time)
vbox_ramp_up.addWidget(self.ramp_up_time_spinbox)
time_hbox.addLayout(vbox_ramp_up)
#
vbox_dwell = QVBoxLayout()
vbox_dwell.addWidget(QLabel("Dwell"))
self.dwell_time_spinbox = QDoubleSpinBox()
self.dwell_time_spinbox.setDecimals(5)
self.dwell_time_spinbox.setRange(0, 10.0)
self.dwell_time_spinbox.setSingleStep(0.1)
self.dwell_time_spinbox.valueChanged.connect(self.on_dwell_time_value)
self.dwell_time_spinbox.setValue(self.dwell_time)
vbox_dwell.addWidget(self.dwell_time_spinbox)
time_hbox.addLayout(vbox_dwell)
vbox_ramp_down = QVBoxLayout()
vbox_ramp_down.addWidget(QLabel("Down"))
self.ramp_down_time_spinbox = QDoubleSpinBox()
self.ramp_down_time_spinbox.setDecimals(5)
self.ramp_down_time_spinbox.setRange(0, 10.0)
self.ramp_down_time_spinbox.setSingleStep(0.1)
self.ramp_down_time_spinbox.valueChanged.connect(self.on_ramp_down_time_value)
self.ramp_down_time_spinbox.setValue(self.ramp_down_time)
vbox_ramp_down.addWidget(self.ramp_down_time_spinbox)
time_hbox.addLayout(vbox_ramp_down)
vbox_hold = QVBoxLayout()
vbox_hold.addWidget(QLabel("Hold"))
self.hold_time_spinbox = QDoubleSpinBox()
self.hold_time_spinbox.setDecimals(5)
self.hold_time_spinbox.setRange(0, 10.0)
self.hold_time_spinbox.setSingleStep(0.1)
self.hold_time_spinbox.valueChanged.connect(self.on_hold_time_value)
self.hold_time_spinbox.setValue(self.hold_time)
vbox_hold.addWidget(self.hold_time_spinbox)
time_hbox.addLayout(vbox_hold)
vbox.addLayout(time_hbox)
# Joints title
title_frame = QFrame()
title_frame.setFrameShape(QFrame.StyledPanel)
title_frame.setFrameShadow(QFrame.Raised)
title_hbox = QHBoxLayout()
title_hbox.addWidget(QLabel("Joints"))
title_hbox.addWidget(QLabel("Start"))
title_hbox.addWidget(QLabel("Ramp Up"))
title_hbox.addWidget(QLabel("Ramp Down"))
title_frame.setLayout(title_hbox)
vbox.addWidget(title_frame)
# Define the widgets for each joint
for i, joint in enumerate(self.chain):
# print i,",",joint
self.joint_widget(vbox, i)
# add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
# Define the connections to the outside world
self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
self.commandPublisher = rospy.Publisher(self.trajectory_topic, JointTrajectory, queue_size=10)
# Add the widget
context.add_widget(self._widget)
@Slot()
def snap_current_callback(self):
self.blockSignals(True)
print "Snapping positions to actual values"
for index, joint in enumerate(self.chain):
for index_state, name_state in enumerate(self.robot_joint_state.name):
if name_state == joint.name:
joint.position = copy.deepcopy(self.robot_joint_state.position[index_state])
self.cmd_spinbox[index].setValue(joint.position)
break
self.blockSignals(False)
@Slot()
def check_limits_callback(self):
self.blockSignals(True)
print "Check limits callback ..."
valid = True
for index, joint in enumerate(self.chain):
ramp_up = joint.position + joint.ramp_up
ramp_down = joint.ramp_down + ramp_up
p = self.ramp_up_spinbox[index].palette()
if (ramp_up > joint.upper_limit) or (ramp_up < joint.lower_limit):
print "Joint ", joint.name, " is over limit on ramp up!"
valid = False
p.setColor(QPalette.Window, Qt.red) # <<<<<<<<<<<----------------- This isn't working as intended
else:
p.setColor(QPalette.Window, Qt.white) # <<<<<<<<<<<----------------- This isn't working as intended
# print joint.lower_limit," < ", ramp_up, " < ",joint.upper_limit
self.ramp_up_spinbox[index].setPalette(p)
if (ramp_down > joint.upper_limit) or (ramp_down < joint.lower_limit):
print "Joint ", joint.name, " is over limit on ramp down!"
valid = False
p.setColor(QPalette.Window, Qt.red) # <<<<<<<<<<<----------------- This isn't working as intended
else:
p.setColor(QPalette.Window, Qt.white) # <<<<<<<<<<<----------------- This isn't working as intended
# print joint.lower_limit," < ", ramp_down, " < ",joint.upper_limit
if not valid:
print "Invalid joint limits on ramp!"
else:
print " Valid ramp!"
self.blockSignals(False)
return valid
@Slot()
def zero_ramp_callback(self):
self.blockSignals(True)
print "Zero ramps callback ..."
for index, joint in enumerate(self.chain):
self.ramp_up_spinbox[index].setValue(0.0)
self.ramp_down_spinbox[index].setValue(0.0)
self.blockSignals(False)
@Slot()
def apply_command_callback(self):
self.blockSignals(True)
print "Send trajectory"
if self.calculate_trajectory():
# print self.robot_command;
self.commandPublisher.publish(self.robot_command)
else:
print "Trajectory calculation failure - do not publish!"
self.blockSignals(False)
@Slot()
def save_trajectory_callback(self):
self.blockSignals(True)
print "Save gains"
# Save data to file that we can read in
# TODO - invalid file names
fileName = QFileDialog.getSaveFileName()
# if fileName[0] checks to ensure that the file dialog has not been canceled
if fileName[0]:
if self.calculate_trajectory():
print self.robot_command
newFileptr = open(fileName[0], "w")
# Suggested format for files to make it easy to combine different outputs
newFileptr.write("# Trajectory \n")
newFileptr.write(self.robot_command)
newFileptr.write("\n")
newFileptr.close()
else:
print "Trajectory calculation failure - do not save!"
else:
print "Save cancelled."
newFilePtr.close()
self.blockSignals(False)
#
@Slot()
def stateCallbackFnc(self, atlasState_msg):
# Handle the state message for actual positions
time = atlasState_msg.header.stamp.to_sec()
if (time - self.prior_time) > 0.02:
# Only update at 50hz
# relay the ROS messages through a Qt Signal to switch to the GUI thread
self.updateStateSignal.emit(atlasState_msg)
self.prior_time = time
# this runs in the Qt GUI thread and can therefore update the GUI
def on_updateState(self, joint_state_msg):
# print joint_state_msg
self.robot_joint_state = joint_state_msg
def on_delay_time_value(self, value):
self.delay_time = copy.deepcopy(value)
def on_ramp_up_time_value(self, value):
self.ramp_up_time = copy.deepcopy(value)
def on_ramp_down_time_value(self, value):
self.ramp_down_time = copy.deepcopy(value)
def on_dwell_time_value(self, value):
self.dwell_time = copy.deepcopy(value)
def on_hold_time_value(self, value):
self.hold_time = copy.deepcopy(value)
def calculate_trajectory(self):
if not self.check_limits_callback():
print "Invalid limits for trajectory!"
return False
knot_points = 1
if self.delay_time > 0.002:
knot_points += 1
if self.ramp_up_time > 0.002:
knot_points += 1
if self.dwell_time > 0.002:
knot_points += 1
if self.ramp_down_time > 0.002:
knot_points += 1
if self.hold_time > 0.002:
knot_points += 1
if knot_points < 2:
print "Invalid trajectory - knot_points = ", knot_points
return False
# print "Minimum knot points=",knot_points
self.robot_command.points = []
self.robot_command.points.append(JointTrajectoryPoint())
ros_time_offset = rospy.Duration(0.0)
ndx = 0
self.robot_command.points[ndx].time_from_start = ros_time_offset
for jnt, joint in enumerate(self.chain):
self.robot_command.points[ndx].positions.append(joint.position)
if self.spline_order > 1:
self.robot_command.points[ndx].velocities.append(0.0)
if self.spline_order > 3:
self.robot_command.points[ndx].accelerations.append(0.0)
ndx += 1
if self.delay_time > 0.002:
ros_time_offset += rospy.Duration(self.delay_time)
self.robot_command.points.append(copy.deepcopy(self.robot_command.points[ndx - 1]))
self.robot_command.points[ndx].time_from_start = ros_time_offset
ndx += 1
if self.ramp_up_time > 0.002:
ramp_up_count = self.calculate_ramp_up_section(ndx)
if ramp_up_count:
ndx += ramp_up_count
ros_time_offset += rospy.Duration(self.ramp_up_time)
else:
return False # Invalid ramp calculation
if self.dwell_time > 0.002:
ros_time_offset += rospy.Duration(self.dwell_time)
self.robot_command.points.append(copy.deepcopy(self.robot_command.points[ndx - 1]))
self.robot_command.points[ndx].time_from_start = ros_time_offset
ndx += 1
if self.ramp_down_time > 0.002:
ramp_dn_count = self.calculate_ramp_down_section(ndx)
if ramp_dn_count > 0:
ndx += ramp_dn_count
ros_time_offset += rospy.Duration(self.ramp_down_time)
else:
return False # Invalid ramp calculation
if self.hold_time > 0.002:
ros_time_offset += rospy.Duration(self.hold_time)
self.robot_command.points.append(copy.deepcopy(self.robot_command.points[ndx - 1]))
self.robot_command.points[ndx].time_from_start = ros_time_offset
ndx += 1
# print self.robot_command
# print "Ndx=",ndx, " of ",knot_points, "=",len(self.robot_command.points)
if ndx != len(self.robot_command.points):
print "Invalid number of knot points - ignore trajectory"
print "Ndx=", ndx, " of ", len(self.robot_command.points), " = ", knot_points
print self.robot_command
return False
self.robot_command.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
return True
def calculate_ramp_up_section(self, ndx):
prior = self.robot_command.points[ndx - 1]
# Create next 3 knot points
self.robot_command.points.append(copy.deepcopy(prior))
if self.spline_order > 1:
# Add transition points
self.robot_command.points.append(copy.deepcopy(prior))
self.robot_command.points.append(copy.deepcopy(prior))
dT0 = self.ramp_start_fraction * self.ramp_up_time
dT1 = (self.ramp_end_fraction - self.ramp_start_fraction) * self.ramp_up_time
dT2 = (1.0 - self.ramp_end_fraction) * self.ramp_up_time
self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
self.robot_command.points[ndx + 1].time_from_start = self.robot_command.points[
ndx
].time_from_start + rospy.Duration(dT1)
self.robot_command.points[ndx + 2].time_from_start = self.robot_command.points[
ndx + 1
].time_from_start + rospy.Duration(dT2)
else:
dT0 = self.ramp_up_time
self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
# Now calculate the interior values for each joint individually
for jnt, joint in enumerate(self.chain):
if joint.ramp_up != 0.0:
starting_position = copy.deepcopy(prior.positions[jnt])
ending_position = starting_position + joint.ramp_up
print "calculating ramp up for ", joint.name, " from ", starting_position, " to ", ending_position, " over ", self.ramp_up_time, " seconds"
if self.spline_order > 1:
# Either quintic or cubic - either way we have transition -> linear -> transition
# for quintic,
# Ax=b where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
# 6 + 2 + 6 unknowns
x = self.solve_ramp_matrices(dT0, dT1, dT2, starting_position, ending_position)
# first interior point
self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(x[7][0])
self.robot_command.points[ndx].velocities[jnt] = copy.deepcopy(x[6][0])
# linear segment
self.robot_command.points[ndx + 1].positions[jnt] = copy.deepcopy(x[13][0])
self.robot_command.points[ndx + 1].velocities[jnt] = copy.deepcopy(x[6][0])
# last interior point
self.robot_command.points[ndx + 2].positions[jnt] = ending_position
self.robot_command.points[ndx + 2].velocities[jnt] = 0.0
else:
# Piecewise linear
self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(ending_position)
if self.spline_order > 3:
# Quintic spline
self.robot_command.points[ndx].accelerations[jnt] = 0.0
self.robot_command.points[ndx + 1].accelerations[jnt] = 0.0
self.robot_command.points[ndx + 2].accelerations[jnt] = 0.0
if self.spline_order < 3:
return 1
return 3 # At least cubic and has 3 transition segments
def calculate_ramp_down_section(self, ndx):
prior = self.robot_command.points[ndx - 1]
# Create next knot points
self.robot_command.points.append(copy.deepcopy(prior))
if self.spline_order > 1:
# Add transition points
self.robot_command.points.append(copy.deepcopy(prior))
self.robot_command.points.append(copy.deepcopy(prior))
dT0 = self.ramp_start_fraction * self.ramp_down_time
dT1 = (self.ramp_end_fraction - self.ramp_start_fraction) * self.ramp_down_time
dT2 = (1.0 - self.ramp_end_fraction) * self.ramp_down_time
self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
self.robot_command.points[ndx + 1].time_from_start = self.robot_command.points[
ndx
].time_from_start + rospy.Duration(dT1)
self.robot_command.points[ndx + 2].time_from_start = self.robot_command.points[
ndx + 1
].time_from_start + rospy.Duration(dT2)
else:
dT0 = self.ramp_down_time
self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
# Now calculate the interior values for each joint individually
for jnt, joint in enumerate(self.chain):
if joint.ramp_up != 0.0:
starting_position = copy.deepcopy(prior.positions[jnt])
ending_position = starting_position + joint.ramp_down
print "calculating ramp down for ", joint.name, " from ", starting_position, " to ", ending_position, " over ", self.ramp_down_time, " seconds"
if self.spline_order > 1:
# Ax=b where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
# 6 + 2 + 6 unknowns
x = self.solve_ramp_matrices(dT0, dT1, dT2, starting_position, ending_position)
# first interior point
self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(x[7][0])
self.robot_command.points[ndx].velocities[jnt] = copy.deepcopy(x[6][0])
# linear segment
self.robot_command.points[ndx + 1].positions[jnt] = copy.deepcopy(x[13][0])
self.robot_command.points[ndx + 1].velocities[jnt] = copy.deepcopy(x[6][0])
# last interior point
self.robot_command.points[ndx + 2].positions[jnt] = ending_position
self.robot_command.points[ndx + 2].velocities[jnt] = 0.0
else:
self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(ending_position)
if self.spline_order > 3:
self.robot_command.points[ndx + 2].accelerations[jnt] = 0.0
self.robot_command.points[ndx].accelerations[jnt] = 0.0
self.robot_command.points[ndx + 1].accelerations[jnt] = 0.0
if self.spline_order < 3:
return 1
return 3 # At least cubic and has 3 transition segments
def solve_ramp_matrices(self, dT0, dT1, dT2, starting_position, ending_position):
if self.spline_order < 4:
# assume cubic spline
return self.solve_ramp_cubic_matrices(dT0, dT1, dT2, starting_position, ending_position)
# This is the quintic version
# Ax=b where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
# 6 + 2 + 6 = 14 unknowns
A = np.zeros((14, 14))
b = np.zeros((14, 1))
# Eqn 1 - Known starting position
A[0][5] = 1
b[0][0] = starting_position
# Eqn 2 - Zero velocity at start
A[1][4] = 1.0
# Eqn 3 - Zero acceleration at start
A[2][3] = 2.0
# Eqn 4 - Continuous velocity at first interior knot point
A[3][0] = 5 * dT0 ** 4
A[3][1] = 4 * dT0 ** 3
A[3][2] = 3 * dT0 ** 2
A[3][3] = 2 * dT0
A[3][4] = 1.0
A[3][5] = 0.0
A[3][6] = -1.0
# Eqn 5 - Zero acceleration at first interior knot point
A[4][0] = 20 * dT0 ** 3
A[4][1] = 12 * dT0 ** 2
A[4][2] = 6 * dT0
A[4][3] = 2
A[4][4] = 0.0
A[4][5] = 0.0
A[4][6] = 0.0
# Eqn 6 - Zero jerk at first interior knot point
A[5][0] = 60 * dT0 ** 2
A[5][1] = 24 * dT0
A[5][2] = 6
A[5][3] = 0.0
A[5][4] = 0.0
A[5][5] = 0.0
A[5][6] = 0.0
# Eqn 7 - Continuous velocity at second interior knot point
A[6][8] = 0.0
A[6][9] = 0.0
A[6][10] = 0.0
A[6][11] = 0.0
A[6][12] = 1.0
A[6][13] = 0.0
A[6][6] = -1.0
# Eqn 8 - Zero acceleration at second interior knot point
A[7][8] = 0.0
A[7][9] = 0.0
A[7][10] = 0.0
A[7][11] = 2
A[7][12] = 0.0
A[7][13] = 0.0
A[7][6] = 0.0
# Eqn 9 - Zero jerk at second interior knot point
A[8][8] = 0.0
A[8][9] = 0.0
A[8][10] = 6
A[8][11] = 0.0
A[8][12] = 0.0
A[8][13] = 0.0
A[8][6] = 0.0
# Eqn 10 - Equal (but unknown) positions at first interior knot point
A[9][0] = dT0 ** 5
A[9][1] = dT0 ** 4
A[9][2] = dT0 ** 3
A[9][3] = dT0 ** 2
A[9][4] = dT0
A[9][5] = 1.0
A[9][7] = -1.0
# Eqn 11 - Equal (but unknown) positions at second interior knot point
A[10][13] = 1.0
A[10][6] = -dT1
A[10][7] = -1.0
# Eqn 12 - Known final position
A[11][8] = dT2 ** 5
A[11][9] = dT2 ** 4
A[11][10] = dT2 ** 3
A[11][11] = dT2 ** 2
A[11][12] = dT2
A[11][13] = 1.0
b[11][0] = ending_position
# Eqn 13 - Known final velocity = 0
A[12][8] = 5 * dT2 ** 4
A[12][9] = 4 * dT2 ** 3
A[12][10] = 3 * dT2 ** 2
A[12][11] = 2 * dT2
A[12][12] = 1.0
A[12][13] = 0.0
# Eqn 14 - Known final acceleration = 0
A[13][8] = 20 * dT2 ** 3
A[13][9] = 12 * dT2 ** 2
A[13][10] = 6 * dT2
A[13][11] = 2.0
A[13][12] = 0.0
A[13][13] = 0.0
params = np.linalg.solve(A, b)
# np.set_printoptions(precision=9, suppress=True, linewidth=250)
# print "A=",A
# print "b=",b
# print "condition number(A)=",np.linalg.cond(A)
# print "x=",params
return params
def solve_ramp_cubic_matrices(self, dT0, dT1, dT2, starting_position, ending_position):
# This is the cubic version
# Quintic version: Ax=b where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
# Cubic version: Ax=b where x=[c0 d0 e0 f0 e1 f1 c2 d2 e2 f2] with p= c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
#
# 4 + 2 + 4 = 10 unknowns
A = np.zeros((10, 10))
b = np.zeros((10, 1))
# Eqn 1 - Known starting position
A[0][3] = 1
b[0][0] = starting_position
# Eqn 2 - Zero velocity at start
A[1][2] = 1.0
# Eqn 3 - Continuous velocity at first interior knot point
A[2][0] = 3 * dT0 ** 2
A[2][1] = 2 * dT0
A[2][2] = 1.0
A[2][3] = 0.0
A[2][4] = -1.0
# Eqn 4 - Zero acceleration at first interior knot point
A[3][0] = 6 * dT0
A[3][1] = 2
A[3][2] = 0.0
A[3][3] = 0.0
# Eqn 5 - Continuous velocity at second interior knot point
A[4][6] = 0.0
A[4][7] = 0.0
A[4][8] = 1.0
A[4][9] = 0.0
A[4][4] = -1.0
# Eqn 6 - Zero acceleration at second interior knot point
A[5][6] = 0.0
A[5][7] = 2.0
A[5][8] = 0.0
A[5][9] = 0.0
# Eqn 7 - Equal (but unknown) positions at first interior knot point
A[6][0] = dT0 ** 3
A[6][1] = dT0 ** 2
A[6][2] = dT0
A[6][3] = 1.0
A[6][5] = -1.0
# Eqn 8 - Equal (but unknown) positions at second interior knot point
A[7][9] = 1.0
A[7][4] = -dT1
A[7][5] = -1.0
# Eqn 9 - Known final position
A[8][6] = dT2 ** 3
A[8][7] = dT2 ** 2
A[8][8] = dT2
A[8][9] = 1.0
b[8][0] = ending_position
# Eqn 10 - Known final velocity = 0
A[9][6] = 3 * dT2 ** 2
A[9][7] = 2 * dT2
A[9][8] = 1.0
A[9][9] = 0.0
params = np.linalg.solve(A, b)
quintic_params = np.zeros((14, 1))
quintic_params[2:8] = params[0:6]
quintic_params[10:14] = params[6:10]
np.set_printoptions(precision=9, suppress=True, linewidth=250)
print "A=", A
print "b=", b
print "condition number(A)=", np.linalg.cond(A)
print "x=", params
# print "-----------------------------------------------------------------------"
# print "--------- Cubic solve -------------------------------------------------"
# print "cubic params=",params
# print "quintic params = ", quintic_params
# print "-----------------------------------------------------------------------"
return quintic_params
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.jointSubscriber.unregister()
self.commandPublisher.unregister()
rospy.sleep(0.5)
print "Done!"
# Define collection of widgets for joint group
def joint_widget(self, main_vbox, index):
joint = self.chain[index]
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel)
frame.setFrameShadow(QFrame.Raised)
hbox = QHBoxLayout()
# hbox.addWidget(frame)
self.prior_time = 0.0
robot_joint = self.robot.joints[self.joint_list[joint.name]]
joint.lower_limit = robot_joint.limit.lower
joint.upper_limit = robot_joint.limit.upper
ramp_range = robot_joint.limit.upper - robot_joint.limit.lower
print " ", joint.name, " limits(", joint.lower_limit, ", ", joint.upper_limit, ") range=", ramp_range
self.cmd_spinbox.append(QDoubleSpinBox())
self.cmd_spinbox[index].setDecimals(5)
self.cmd_spinbox[index].setRange(joint.lower_limit, joint.upper_limit)
self.cmd_spinbox[index].setSingleStep((robot_joint.limit.upper - robot_joint.limit.lower) / 50.0)
self.cmd_spinbox[index].valueChanged.connect(joint.on_cmd_value)
self.ramp_up_spinbox.append(QDoubleSpinBox())
self.ramp_up_spinbox[index].setDecimals(5)
self.ramp_up_spinbox[index].setRange(-ramp_range, ramp_range)
self.ramp_up_spinbox[index].setSingleStep(ramp_range / 50.0)
self.ramp_up_spinbox[index].valueChanged.connect(joint.on_ramp_up_value)
self.ramp_down_spinbox.append(QDoubleSpinBox())
self.ramp_down_spinbox[index].setDecimals(5)
self.ramp_down_spinbox[index].setRange(-ramp_range, ramp_range)
self.ramp_down_spinbox[index].setSingleStep(ramp_range / 50.0)
self.ramp_down_spinbox[index].valueChanged.connect(joint.on_ramp_down_value)
hbox.addWidget(QLabel(joint.name))
hbox.addWidget(self.cmd_spinbox[index])
hbox.addWidget(self.ramp_up_spinbox[index])
hbox.addWidget(self.ramp_down_spinbox[index])
# Add horizontal layout to frame for this joint group
frame.setLayout(hbox)
# Add frame to main vertical layout
main_vbox.addWidget(frame)
