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))
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 )
def __init__(self, context):
super(HeadControlDialog, self).__init__(context)
self.setObjectName('HeadControlDialog')
self.head_max_pitch_deg = 65.0
self.head_min_pitch_deg = -60.0
self.head_pitch_range_deg = self.head_max_pitch_deg - self.head_min_pitch_deg
self.scan_period_secs = 20.0
self.head_pitch_cmd_deg = 0.0
self.scan_offset = rospy.get_rostime()
self.last_publish_time = self.scan_offset
self._timer = None
self._widget = QWidget()
vbox = QVBoxLayout()
spindle_speed_hbox = QHBoxLayout()
#Add input for setting the spindle speed
spindle_label = QLabel("Spindle Speed [deg/s]")
spindle_speed_hbox.addWidget(spindle_label)
#Add a spinbox for setting the spindle speed
spindle_speed_spinbox = QDoubleSpinBox()
spindle_speed_spinbox.setRange(-360 * 4, 360 * 4)
spindle_speed_spinbox.valueChanged.connect(self.handle_spindle_speed)
self.spindle_speed_pub = rospy.Publisher('/multisense/set_spindle_speed', Float64, queue_size=10)
spindle_speed_hbox.addWidget(spindle_speed_spinbox)
vbox.addLayout(spindle_speed_hbox)
#Add input for directly setting the head pitch
head_pitch_hbox = QHBoxLayout()
head_pitch_label = QLabel("Head Pitch [deg]")
head_pitch_hbox.addWidget(head_pitch_label)
#Add a spinbox for setting the head pitch directly
self.head_pitch_spinbox = QDoubleSpinBox()
self.head_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
self.head_pitch_spinbox.valueChanged.connect(self.handle_head_pitch)
self.head_pitch_pub = rospy.Publisher('/atlas/pos_cmd/neck_ry', Float64, queue_size=10)
head_pitch_hbox.addWidget(self.head_pitch_spinbox)
vbox.addLayout(head_pitch_hbox)
#Publisher for head trajectory
self.head_trajectory_pub = rospy.Publisher('/trajectory_controllers/neck_traj_controller/command', JointTrajectory, queue_size=10)
#Add checkbox for invoking scanning behavior
self.head_scan_chkbox = QCheckBox("Scanning behavior")
self.head_scan_chkbox.stateChanged.connect(self.handle_scan_chg)
vbox.addWidget(self.head_scan_chkbox)
#Add input for setting the minimum head pitch
head_min_pitch_hbox = QHBoxLayout()
head_min_pitch_label = QLabel("Min Head Pitch [deg] (raise head)")
head_min_pitch_hbox.addWidget(head_min_pitch_label)
head_min_pitch_spinbox = QDoubleSpinBox()
head_min_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
head_min_pitch_spinbox.setValue(self.head_min_pitch_deg)
head_min_pitch_spinbox.valueChanged.connect(self.handle_head_min_pitch)
head_min_pitch_hbox.addWidget(head_min_pitch_spinbox)
vbox.addLayout(head_min_pitch_hbox)
#Add input for setting the maximum head pitch
head_max_pitch_hbox = QHBoxLayout()
head_max_pitch_label = QLabel("Max Head Pitch [deg] (lower head)")
head_max_pitch_hbox.addWidget(head_max_pitch_label)
head_max_pitch_spinbox = QDoubleSpinBox()
head_max_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
head_max_pitch_spinbox.setValue(self.head_max_pitch_deg)
head_max_pitch_spinbox.valueChanged.connect(self.handle_head_max_pitch)
head_max_pitch_hbox.addWidget(head_max_pitch_spinbox)
vbox.addLayout(head_max_pitch_hbox)
#Add input for setting the scan period
head_period_hbox = QHBoxLayout()
head_period_label = QLabel("Scanning Period [secs]")
head_period_hbox.addWidget(head_period_label)
head_period_spinbox = QDoubleSpinBox()
head_period_spinbox.setRange(0.01, 60.0)
head_period_spinbox.setValue(self.scan_period_secs)
head_period_spinbox.valueChanged.connect(self.handle_head_period)
head_period_hbox.addWidget(head_period_spinbox)
vbox.addLayout(head_period_hbox)
#add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
class HeadControlDialog(Plugin):
def __init__(self, context):
super(HeadControlDialog, self).__init__(context)
self.setObjectName('HeadControlDialog')
self.head_max_pitch_deg = 65.0
self.head_min_pitch_deg = -60.0
self.head_pitch_range_deg = self.head_max_pitch_deg - self.head_min_pitch_deg
self.scan_period_secs = 20.0
self.head_pitch_cmd_deg = 0.0
self.scan_offset = rospy.get_rostime()
self.last_publish_time = self.scan_offset
self._timer = None
self._widget = QWidget()
vbox = QVBoxLayout()
spindle_speed_hbox = QHBoxLayout()
#Add input for setting the spindle speed
spindle_label = QLabel("Spindle Speed [deg/s]")
spindle_speed_hbox.addWidget(spindle_label)
#Add a spinbox for setting the spindle speed
spindle_speed_spinbox = QDoubleSpinBox()
spindle_speed_spinbox.setRange(-360 * 4, 360 * 4)
spindle_speed_spinbox.valueChanged.connect(self.handle_spindle_speed)
self.spindle_speed_pub = rospy.Publisher('/multisense/set_spindle_speed', Float64, queue_size=10)
spindle_speed_hbox.addWidget(spindle_speed_spinbox)
vbox.addLayout(spindle_speed_hbox)
#Add input for directly setting the head pitch
head_pitch_hbox = QHBoxLayout()
head_pitch_label = QLabel("Head Pitch [deg]")
head_pitch_hbox.addWidget(head_pitch_label)
#Add a spinbox for setting the head pitch directly
self.head_pitch_spinbox = QDoubleSpinBox()
self.head_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
self.head_pitch_spinbox.valueChanged.connect(self.handle_head_pitch)
self.head_pitch_pub = rospy.Publisher('/atlas/pos_cmd/neck_ry', Float64, queue_size=10)
head_pitch_hbox.addWidget(self.head_pitch_spinbox)
vbox.addLayout(head_pitch_hbox)
#Publisher for head trajectory
self.head_trajectory_pub = rospy.Publisher('/trajectory_controllers/neck_traj_controller/command', JointTrajectory, queue_size=10)
#Add checkbox for invoking scanning behavior
self.head_scan_chkbox = QCheckBox("Scanning behavior")
self.head_scan_chkbox.stateChanged.connect(self.handle_scan_chg)
vbox.addWidget(self.head_scan_chkbox)
#Add input for setting the minimum head pitch
head_min_pitch_hbox = QHBoxLayout()
head_min_pitch_label = QLabel("Min Head Pitch [deg] (raise head)")
head_min_pitch_hbox.addWidget(head_min_pitch_label)
head_min_pitch_spinbox = QDoubleSpinBox()
head_min_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
head_min_pitch_spinbox.setValue(self.head_min_pitch_deg)
head_min_pitch_spinbox.valueChanged.connect(self.handle_head_min_pitch)
head_min_pitch_hbox.addWidget(head_min_pitch_spinbox)
vbox.addLayout(head_min_pitch_hbox)
#Add input for setting the maximum head pitch
head_max_pitch_hbox = QHBoxLayout()
head_max_pitch_label = QLabel("Max Head Pitch [deg] (lower head)")
head_max_pitch_hbox.addWidget(head_max_pitch_label)
head_max_pitch_spinbox = QDoubleSpinBox()
head_max_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
head_max_pitch_spinbox.setValue(self.head_max_pitch_deg)
head_max_pitch_spinbox.valueChanged.connect(self.handle_head_max_pitch)
head_max_pitch_hbox.addWidget(head_max_pitch_spinbox)
vbox.addLayout(head_max_pitch_hbox)
#Add input for setting the scan period
head_period_hbox = QHBoxLayout()
head_period_label = QLabel("Scanning Period [secs]")
head_period_hbox.addWidget(head_period_label)
head_period_spinbox = QDoubleSpinBox()
head_period_spinbox.setRange(0.01, 60.0)
head_period_spinbox.setValue(self.scan_period_secs)
head_period_spinbox.valueChanged.connect(self.handle_head_period)
head_period_hbox.addWidget(head_period_spinbox)
vbox.addLayout(head_period_hbox)
#add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def shutdown_plugin(self):
print "Shutdown ..."
if self._timer is not None:
self._timer.shutdown()
rospy.sleep(0.1)
self.spindle_speed_pub.unregister()
self.head_pitch_pub.unregister()
self.head_trajectory_pub.unregister()
print "Done!"
#Slot for setting the spindle speed
def handle_spindle_speed(self, degree_per_sec):
self.spindle_speed_pub.publish(data=math.radians(degree_per_sec))
#Slot for setting the head pitch state
def handle_head_pitch(self, pitch_degree):
self.head_pitch_cmd_deg = pitch_degree
self.head_pitch_pub.publish(data=math.radians(pitch_degree))
#Publish neck trajectory
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = ['neck_ry']
trajectory.points = [JointTrajectoryPoint()]
trajectory.points[0].positions = [math.radians(pitch_degree)]
trajectory.points[0].velocities = [0.0]
trajectory.points[0].time_from_start = rospy.Duration(0.75)
self.head_trajectory_pub.publish(trajectory)
#Slot for setting the head max pitch state
def handle_head_max_pitch(self, value):
self.head_max_pitch_deg = value
self.head_pitch_range_deg = self.head_max_pitch_deg - self.head_min_pitch_deg
if (self.head_pitch_range_deg < 0.0001):
print 'invalid pitch limits - fix range!'
self.head_pitch_range_deg = 0.01
#Slot for setting the head min pitch state
def handle_head_min_pitch(self, value):
self.head_min_pitch_deg = value
self.head_pitch_range_deg = self.head_max_pitch_deg - self.head_min_pitch_deg
if (self.head_pitch_range_deg < 0.0001):
print 'invalid pitch limits - fix range!'
self.head_pitch_range_deg = 0.01
def handle_head_period(self, value):
self.scan_period_secs = value
@Slot(bool)
def handle_scan_chg(self, checked):
ros_time = rospy.get_rostime()
if checked:
print 'Start scanning ...'
if ((self.head_pitch_cmd_deg >= self.head_min_pitch_deg) and (self.head_pitch_cmd_deg <= self.head_max_pitch_deg)):
# adjust offset so that scanning command starts from current angle
# to provide bumpless transfer
# cos value corresponding to the current head pitch command
tmp = ((self.head_pitch_cmd_deg - self.head_min_pitch_deg) / self.head_pitch_range_deg - 0.5) * 2.0
# angle to give the same cosine value as current command
tmp_rad = math.acos(tmp)
tmp_time = tmp_rad * self.scan_period_secs / (math.pi * 2.0)
#Use integer math to avoid issues with UTC time having large numbers
self.scan_offset.secs = int(math.floor(tmp_time))
self.scan_offset.nsecs = int(math.floor((tmp_time - self.scan_offset.secs) * 1000000000))
self.scan_offset.secs -= ros_time.secs
self.scan_offset.nsecs -= ros_time.nsecs
if (self.scan_offset.nsecs < 0):
self.scan_offset.secs -= 1
self.scan_offset.nsecs += 1000000000
else:
print 'outside of range - ignore offset '
self.scan_offset = ros_time
self._timer = rospy.Timer(rospy.Duration(0.01), self.time_callback)
else:
print 'Stop scanning!'
if self._timer is not None:
self._timer.shutdown()
#Update the time
def time_callback(self, timer_event):
ros_time = rospy.get_rostime()
# Test and only publish at 100Hz so often to avoid spamming at 1khz
delta_time = ros_time - self.last_publish_time
if (delta_time.secs > 0) or (delta_time.nsecs > 10000000):
self.last_publish_time = ros_time
float_time = float(ros_time.secs + self.scan_offset.secs) + float(ros_time.nsecs+ self.scan_offset.nsecs) * 1e-9
rad = (float_time) * (math.pi * 2.0 / self.scan_period_secs)
cos_rad = math.cos(rad)
frac_range = 0.5 * cos_rad + 0.5
tmp_deg = frac_range * self.head_pitch_range_deg + self.head_min_pitch_deg
# Set value and publish inside spinbox callback
self.head_pitch_spinbox.setValue(tmp_deg)
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"
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)
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()
