diff --git a/body/stretch_body/pimu.py b/body/stretch_body/pimu.py index 676e1d18..8ae6fa8c 100644 --- a/body/stretch_body/pimu.py +++ b/body/stretch_body/pimu.py @@ -56,58 +56,45 @@ def pretty_print(self): print('Timestamp (s)', self.status['timestamp']) print('-----------------------') - def unpack_status(self, s): - raise NotImplementedError('This method not supported by IMU for firmware.') + def unpack_status(self, s, unpack_to=None): + if unpack_to is None: + unpack_to = self.status + sidx = 0 + unpack_to['ax'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['ay'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['az'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['gx'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['gy'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['gz'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['mx'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['my'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['mz'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['roll'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4 + unpack_to['pitch'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4 + unpack_to['heading'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4 + unpack_to['qw'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['qx'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['qy'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['qz'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['bump'] = unpack_float_t(s[sidx:]);sidx += 4 + return sidx + # ######################## IMU PROTOCOL P0 ################################# class IMU_Protocol_P0(IMUBase): - def unpack_status(self, s): - # take in an array of bytes - # this needs to exactly match the C struct format - sidx=0 - self.status['ax']= unpack_float_t(s[sidx:]);sidx += 4 - self.status['ay'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['az'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['gx'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['gy'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['gz'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['mx'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['my'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['mz'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['roll'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4 - self.status['pitch'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4 - self.status['heading'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4 - self.status['qw'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['qx'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['qy'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['qz'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['bump'] = unpack_float_t(s[sidx:]);sidx += 4 + def unpack_status(self, s, unpack_to=None): + if unpack_to is None: + unpack_to = self.status + sidx = IMUBase.unpack_status(self, s, unpack_to) self.status['timestamp'] = self.timestamp.set(unpack_uint32_t(s[sidx:]));sidx += 4 return sidx # ######################## IMU PROTOCOL P1 ################################# class IMU_Protocol_P1(IMUBase): - def unpack_status(self, s): - # take in an array of bytes - # this needs to exactly match the C struct format - sidx=0 - self.status['ax']= unpack_float_t(s[sidx:]);sidx += 4 - self.status['ay'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['az'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['gx'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['gy'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['gz'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['mx'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['my'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['mz'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['roll'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4 - self.status['pitch'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4 - self.status['heading'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4 - self.status['qw'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['qx'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['qy'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['qz'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['bump'] = unpack_float_t(s[sidx:]);sidx += 4 + def unpack_status(self, s, unpack_to=None): + if unpack_to is None: + unpack_to = self.status + sidx = IMUBase.unpack_status(self, s, unpack_to) return sidx # ######################## IMU ################################# @@ -116,7 +103,8 @@ def __init__(self): IMUBase.__init__(self) # Order in descending order so more recent protocols/methods override less recent self.supported_protocols = {'p0': (IMU_Protocol_P0,), 'p1': (IMU_Protocol_P1,IMU_Protocol_P0,), - 'p2': (IMU_Protocol_P1,IMU_Protocol_P0,),'p3': (IMU_Protocol_P1,IMU_Protocol_P0,)} + 'p2': (IMU_Protocol_P1,IMU_Protocol_P0,),'p3': (IMU_Protocol_P1,IMU_Protocol_P0,), + 'p4': (IMU_Protocol_P1,IMU_Protocol_P0,)} # ################################################################################## class PimuBase(Device): @@ -193,7 +181,7 @@ def __init__(self, event_reset=False, usb=None): 'cliff_event': False, 'fan_on': False, 'buzzer_on': False, 'low_voltage_alert':False,'high_current_alert':False,'over_tilt_alert':False, 'charger_connected':False, 'boot_detected':False,'imu': self.imu.status,'debug':0,'state':0,'trace_on':0, 'motor_sync_rate': 0, 'motor_sync_cnt': 0, 'motor_sync_queues': 0, 'motor_sync_drop': 0, - 'transport': self.transport.status} + 'transport': self.transport.status, 'current_charge':0} self.status_zero=self.status.copy() self.status_aux = {'foo': 0} @@ -294,6 +282,8 @@ def pretty_print(self): print('------ Pimu -----') print('Voltage',self.status['voltage']) print('Current', self.status['current']) + if self.board_info['hardware_id']>=3: + print('Current Charge',self.status['current_charge']) print('CPU Temp',self.status['cpu_temp']) print('Board Temp', self.status['temp']) print('State', self.status['state']) @@ -422,10 +412,36 @@ def get_temp(self,raw): return C def get_current(self,raw): + if self.board_info['hardware_id']>=3: + return self.get_current_efuse(raw) + else: + return self.get_current_shunt(raw) + def get_current_shunt(self,raw): + """ + RE1 / RE2 Pimus using shunt resistor for current measurement + """ raw_to_mV = 3300 / 1024.0 mV = raw * raw_to_mV mA = mV/.408 # conversion per circuit return mA/1000.0 + + def get_current_efuse(self,raw): + """ + S3 Pimu's using Efuse current measurement + """ + raw_to_mV = 3300 / 1024.0 + mV = raw * raw_to_mV + mA = mV/.224 # conversion per circuit + return mA/1000.0 + + def get_current_charge(self,raw): + """ + S3 Pimu's shunt measurement of charger current + """ + raw_to_mV = 3300 / 1024.0 + mV = raw * raw_to_mV + mA = mV/.215 # conversion per circuit + return mA/1000.0 # ################Data Packing ##################### def unpack_board_info(self,s): @@ -476,7 +492,7 @@ def pack_trigger(self,s,sidx): pack_uint32_t(s,sidx,self._trigger); sidx+=4 return sidx - def unpack_status(self,s): + def unpack_status(self,s, unpack_to=None): raise NotImplementedError('This method not supported for firmware on protocol {0}.' .format(self.board_info['protocol_version'])) @@ -579,38 +595,41 @@ def step_sentry(self,robot=None): # ######################## PIMU PROTOCOL PO ################################# + class Pimu_Protocol_P0(PimuBase): - def unpack_status(self,s): + def unpack_status(self,s, unpack_to=None): + if unpack_to is None: + unpack_to = self.status sidx=0 sidx +=self.imu.unpack_status((s[sidx:])) - self.status['voltage']=self.get_voltage(unpack_float_t(s[sidx:]));sidx+=4 - self.status['current'] = self.get_current(unpack_float_t(s[sidx:]));sidx+=4 - self.status['temp'] = self.get_temp(unpack_float_t(s[sidx:]));sidx+=4 + unpack_to['voltage']=self.get_voltage(unpack_float_t(s[sidx:]));sidx+=4 + unpack_to['current'] = self.get_current(unpack_float_t(s[sidx:]));sidx+=4 + unpack_to['temp'] = self.get_temp(unpack_float_t(s[sidx:]));sidx+=4 for i in range(4): - self.status['cliff_range'][i]=unpack_float_t(s[sidx:]) + unpack_to['cliff_range'][i]=unpack_float_t(s[sidx:]) sidx+=4 - self.status['state'] = unpack_uint32_t(s[sidx:]) + unpack_to['state'] = unpack_uint32_t(s[sidx:]) sidx += 4 - self.status['at_cliff']=[] - self.status['at_cliff'].append((self.status['state'] & self.STATE_AT_CLIFF_0) != 0) - self.status['at_cliff'].append((self.status['state'] & self.STATE_AT_CLIFF_1) != 0) - self.status['at_cliff'].append((self.status['state'] & self.STATE_AT_CLIFF_2) != 0) - self.status['at_cliff'].append((self.status['state'] & self.STATE_AT_CLIFF_3) != 0) - self.status['runstop_event'] = (self.status['state'] & self.STATE_RUNSTOP_EVENT) != 0 - self.status['cliff_event'] = (self.status['state'] & self.STATE_CLIFF_EVENT) != 0 - self.status['fan_on'] = (self.status['state'] & self.STATE_FAN_ON) != 0 - self.status['buzzer_on'] = (self.status['state'] & self.STATE_BUZZER_ON) != 0 - self.status['low_voltage_alert'] = (self.status['state'] & self.STATE_LOW_VOLTAGE_ALERT) != 0 - self.status['high_current_alert'] = (self.status['state'] & self.STATE_HIGH_CURRENT_ALERT) != 0 - self.status['over_tilt_alert'] = (self.status['state'] & self.STATE_OVER_TILT_ALERT) != 0 - self.status['charger_connected'] = (self.status['state'] & self.STATE_CHARGER_CONNECTED) != 0 - self.status['boot_detected'] = (self.status['state'] & self.STATE_BOOT_DETECTED) != 0 - self.status['timestamp'] = self.timestamp.set(unpack_uint32_t(s[sidx:])); sidx += 4 - self.status['bump_event_cnt'] = unpack_uint16_t(s[sidx:]);sidx += 2 - self.status['debug'] = unpack_float_t(s[sidx:]); sidx += 4 + unpack_to['at_cliff']=[] + unpack_to['at_cliff'].append((unpack_to['state'] & self.STATE_AT_CLIFF_0) != 0) + unpack_to['at_cliff'].append((unpack_to['state'] & self.STATE_AT_CLIFF_1) != 0) + unpack_to['at_cliff'].append((unpack_to['state'] & self.STATE_AT_CLIFF_2) != 0) + unpack_to['at_cliff'].append((unpack_to['state'] & self.STATE_AT_CLIFF_3) != 0) + unpack_to['runstop_event'] = (unpack_to['state'] & self.STATE_RUNSTOP_EVENT) != 0 + unpack_to['cliff_event'] = (unpack_to['state'] & self.STATE_CLIFF_EVENT) != 0 + unpack_to['fan_on'] = (unpack_to['state'] & self.STATE_FAN_ON) != 0 + unpack_to['buzzer_on'] = (unpack_to['state'] & self.STATE_BUZZER_ON) != 0 + unpack_to['low_voltage_alert'] = (unpack_to['state'] & self.STATE_LOW_VOLTAGE_ALERT) != 0 + unpack_to['high_current_alert'] = (unpack_to['state'] & self.STATE_HIGH_CURRENT_ALERT) != 0 + unpack_to['over_tilt_alert'] = (unpack_to['state'] & self.STATE_OVER_TILT_ALERT) != 0 + unpack_to['charger_connected'] = (unpack_to['state'] & self.STATE_CHARGER_CONNECTED) != 0 + unpack_to['boot_detected'] = (unpack_to['state'] & self.STATE_BOOT_DETECTED) != 0 + unpack_to['timestamp'] = self.timestamp.set(unpack_uint32_t(s[sidx:])); sidx += 4 + unpack_to['bump_event_cnt'] = unpack_uint16_t(s[sidx:]);sidx += 2 + unpack_to['debug'] = unpack_float_t(s[sidx:]); sidx += 4 return sidx # ######################## PIMU PROTOCOL P1 ################################# @@ -655,8 +674,6 @@ def unpack_status(self, s, unpack_to=None): - - # ######################## PIMU PROTOCOL P2 ################################# class Pimu_Protocol_P2(PimuBase): @@ -713,7 +730,7 @@ def disable_firmware_trace(self): self._trigger = self._trigger | self.TRIGGER_DISABLE_TRACE self._dirty_trigger = True - def unpack_status(self, s, unpack_to=None): + def unpack_status(self, s, unpack_to=None): #P2 if unpack_to is None: unpack_to = self.status @@ -752,7 +769,7 @@ def unpack_status(self, s, unpack_to=None): unpack_to['debug'] = unpack_float_t(s[sidx:]); sidx += 4 return sidx -# ######################## PIMU PROTOCOL P2 ################################# +# ######################## PIMU PROTOCOL P3 ################################# class Pimu_Protocol_P3(PimuBase): @@ -844,6 +861,29 @@ def rpc_load_test_pull_reply(self, reply): print('Successful load test pull') else: print('Error RPC_REPLY_LOAD_TEST_PULL', reply[0]) + +# ######################## PIMU PROTOCOL P4 ################################# +class Pimu_Protocol_P4(PimuBase): + def unpack_status(self, s, unpack_to=None): # P4 + if unpack_to is None: + unpack_to = self.status + sidx = 0 + sidx = sidx + Pimu_Protocol_P2.unpack_status(self, s, unpack_to) + self.status['current_charge'] = self.get_current_charge(unpack_float_t(s[sidx:]));sidx += 4 + # unpack_to['voltage'] = self.get_voltage(unpack_float_t(s[sidx:])); + # sidx += 4 + return sidx + # def pack_gains(self, s, sidx): + # sidx = sidx + Stepper_Protocol_P3.pack_gains(self, s, sidx) + # pack_float_t(s, sidx, self.gains['voltage_LPF']); + # sidx += 4 + # return sidx + # + # def unpack_gains(self, s): + # sidx = Stepper_Protocol_P3.unpack_gains(self, s) + # self.gains_flash['voltage_LPF'] = unpack_float_t(s[sidx:]); + # sidx += 4 + # return sidx # ######################## PIMU ################################# class Pimu(PimuBase): """ @@ -854,7 +894,8 @@ def __init__(self, event_reset=False, usb=None): # Order in descending order so more recent protocols/methods override less recent self.supported_protocols = {'p0': (Pimu_Protocol_P0,), 'p1': (Pimu_Protocol_P1, Pimu_Protocol_P0,), 'p2': (Pimu_Protocol_P2, Pimu_Protocol_P1, Pimu_Protocol_P0,), - 'p3': (Pimu_Protocol_P3, Pimu_Protocol_P2, Pimu_Protocol_P1, Pimu_Protocol_P0,)} + 'p3': (Pimu_Protocol_P3, Pimu_Protocol_P2, Pimu_Protocol_P1, Pimu_Protocol_P0,), + 'p4': (Pimu_Protocol_P4, Pimu_Protocol_P3, Pimu_Protocol_P2, Pimu_Protocol_P1, Pimu_Protocol_P0,)} def startup(self, threaded=False): """ diff --git a/body/stretch_body/robot_params.py b/body/stretch_body/robot_params.py index f3fad007..67fcaddb 100644 --- a/body/stretch_body/robot_params.py +++ b/body/stretch_body/robot_params.py @@ -62,7 +62,11 @@ class RobotParams: _user_params = hello_utils.read_fleet_yaml('stretch_user_params.yaml') _config_params = hello_utils.read_fleet_yaml('stretch_configuration_params.yaml') _robot_params=nominal_system_params - param_module_name = 'stretch_body.robot_params_'+_config_params['robot']['model_name'] + if 'robot' in _user_params and 'model_name' in _user_params['robot']: + param_module_name = 'stretch_body.robot_params_'+_user_params['robot']['model_name'] + else: + param_module_name = 'stretch_body.robot_params_' + _config_params['robot']['model_name'] + _nominal_params = getattr(importlib.import_module(param_module_name), 'nominal_params') hello_utils.overwrite_dict(_robot_params, _nominal_params) for external_params_module in _config_params.get('params', []): diff --git a/body/stretch_body/robot_params_RE1V0.py b/body/stretch_body/robot_params_RE1V0.py index 27df05ed..40aebc8d 100644 --- a/body/stretch_body/robot_params_RE1V0.py +++ b/body/stretch_body/robot_params_RE1V0.py @@ -267,6 +267,7 @@ 'pKi_limit': 150, 'pKp_d': 8.0, 'pLPF': 60, + 'voltage_LPF':1.0, 'phase_advance_d': 1.8, 'pos_near_setpoint_d': 2.0, 'safety_hold': 0, @@ -303,6 +304,7 @@ 'pKi_limit': 200, 'pKp_d': 8.0, 'pLPF': 200, + 'voltage_LPF':1.0, 'phase_advance_d': 1.8, 'pos_near_setpoint_d': 1.0, 'safety_hold': 0, @@ -339,6 +341,7 @@ 'pKi_limit': 100.0, 'pKp_d': 10.0, 'pLPF': 60, + 'voltage_LPF':1.0, 'phase_advance_d': 1.8, 'pos_near_setpoint_d': 6.0, 'safety_hold': 1, @@ -375,6 +378,7 @@ 'pKi_limit': 200, 'pKp_d': 8.0, 'pLPF': 200, + 'voltage_LPF':1.0, 'phase_advance_d': 1.8, 'pos_near_setpoint_d': 1.0, 'safety_hold': 0, diff --git a/body/stretch_body/robot_params_RE2V0.py b/body/stretch_body/robot_params_RE2V0.py index fb8f7fee..81734cec 100644 --- a/body/stretch_body/robot_params_RE2V0.py +++ b/body/stretch_body/robot_params_RE2V0.py @@ -264,6 +264,7 @@ 'pKi_limit': 150, 'pKp_d': 8.0, 'pLPF': 60, + 'voltage_LPF':1.0, 'phase_advance_d': 1.8, 'pos_near_setpoint_d': 2.0, 'safety_hold': 0, @@ -300,6 +301,7 @@ 'pKi_limit': 200.0, 'pKp_d': 12.0, 'pLPF': 80.0, + 'voltage_LPF':1.0, 'phase_advance_d': 1.8, 'pos_near_setpoint_d': 1.0, 'safety_hold': 0, @@ -336,6 +338,7 @@ 'pKi_limit': 100.0, 'pKp_d': 6.0, 'pLPF': 100, + 'voltage_LPF':1.0, 'phase_advance_d': 1.8, 'pos_near_setpoint_d': 6.0, 'safety_hold': 1, @@ -372,6 +375,7 @@ 'pKi_limit': 200.0, 'pKp_d': 12.0, 'pLPF': 80.0, + 'voltage_LPF':1.0, 'phase_advance_d': 1.8, 'pos_near_setpoint_d': 1.0, 'safety_hold': 0, diff --git a/body/stretch_body/robot_params_SE3.py b/body/stretch_body/robot_params_SE3.py new file mode 100644 index 00000000..77f3c088 --- /dev/null +++ b/body/stretch_body/robot_params_SE3.py @@ -0,0 +1,608 @@ +#Robot parameters for Stretch 3 + +# ######################### USER PARAMS ################################################## +#Template for the generated file: stretch_user_params.yaml +user_params_header='#User parameters\n' \ + '#You can override nominal settings here\n' \ + '#USE WITH CAUTION. IT IS POSSIBLE TO CAUSE UNSAFE BEHAVIOR OF THE ROBOT \n' + +user_params_template={ + 'robot': {'use_collision_manager': 0}} #Include this just as an example + +# ###################### CONFIGURATION PARAMS ##################################################### +#Template for the generated file: stretch_configuration_params.yaml +#Configuration parameters may have variation across the fleet of RE2 robots +configuration_params_header='#Parameters that are specific to this robot\n' \ + '#Do not edit, instead edit stretch_user_params.yaml\n' + +configuration_params_template={ + 'arm':{ + 'contact_models':{ + 'effort_pct':{ + 'contact_thresh_default':[-55.0, 55.0], + 'contact_thresh_homing':[-55.0, 55.0]}}, + 'range_m':[0.0,0.52]}, + 'lift': { + 'contact_models':{ + 'effort_pct': { + 'contact_thresh_default': [-10.0, 69.0], + 'contact_thresh_homing': [-10.0, 69.0]}}, + 'i_feedforward': 1.2, + 'range_m': [0.0, 1.10]}, + 'base':{ + 'wheel_separation_m': 0.3153}, + 'head_pan':{ + 'range_t': [0, 3827], + 'zero_t': 1250}, + 'head_tilt':{ + 'range_t': [1775,3150], + 'zero_t': 2048}, + 'hello-motor-arm':{'serial_no': 'NA'}, + 'hello-motor-lift':{'serial_no': 'NA','gains':{'i_safety_feedforward':1.2}}, + 'hello-motor-left-wheel':{'serial_no': 'NA'}, + 'hello-motor-right-wheel':{'serial_no': 'NA'}, + 'pimu':{ + 'config':{ + 'cliff_zero': [0.0, 0.0, 0.0, 0.0], + 'gyro_zero_offsets': [0.0, 0.0, 0.0], + 'gravity_vector_scale': 1.0, + 'mag_offsets': [0.0, 0.0, 0.0], + 'mag_softiron_matrix': [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + 'rate_gyro_vector_scale': 1.0}}, + 'robot':{ + 'batch_name': 'NA', + 'serial_no': 'NA', + 'd435i':{'serial_no':'NA'}, + 'model_name':'SE3'}, + 'stretch_gripper':{ + 'range_t': [0, 8022], + 'zero_t': 5212}, + 'wacc':{'config':{ + 'accel_gravity_scale': 1.0}}, + 'wrist_yaw':{ + 'range_t': [0,9340], + 'zero_t': 7005}} + +# ###################### NOMINAL PARAMS ##################################################### +#Parameters that are common across the S3 fleet +nominal_params={ + 'arm':{ + 'usb_name': '/dev/hello-motor-arm', + 'force_N_per_A': 55.9, # Legacy + 'chain_pitch': 0.0167, + 'chain_sprocket_teeth': 10, + 'gr_spur': 3.875, + 'i_feedforward': 0, + 'calibration_range_bounds':[0.514, 0.525], + 'contact_models':{ + 'effort_pct': {'contact_thresh_calibration_margin':10.0,'contact_thresh_max': [-90.0, 90.0]}}, + 'motion':{ + 'default':{ + 'accel_m': 0.14, + 'vel_m': 0.14}, + 'fast':{ + 'accel_m': 0.3, + 'vel_m': 0.3}, + 'max':{ + 'accel_m': 0.4, + 'vel_m': 0.4}, + 'slow':{ + 'accel_m': 0.05, + 'vel_m': 0.05}, + 'trajectory_max': { + 'vel_m': 0.4, + 'accel_m': 0.4}}}, + 'base':{ + 'usb_name_left_wheel': '/dev/hello-motor-left-wheel', + 'usb_name_right_wheel': '/dev/hello-motor-right-wheel', + 'force_N_per_A': 21.18, # Legacy + 'gr': 3.8, + 'motion':{ + 'default':{ + 'accel_m': 0.12, + 'vel_m': 0.12}, + 'fast':{ + 'accel_m': 0.25, + 'vel_m': 0.25}, + 'max':{ + 'accel_m': 0.3, + 'vel_m': 0.3}, + 'slow':{ + 'accel_m': 0.06, + 'vel_m': 0.04}, + 'trajectory_max': { + 'vel_r': 50.0, + 'accel_r': 30.0}}, + 'contact_models':{ + 'effort_pct': { + 'contact_thresh_translate_default':60.0, + 'contact_thresh_rotate_default':60.0, + 'contact_thresh_translate_max': 100.0, + 'contact_thresh_rotate_max': 100.0}}, + 'sentry_max_velocity':{ + 'limit_accel_m': 0.15, + 'limit_vel_m': 0.1, + 'max_arm_extension_m': 0.03, + 'max_lift_height_m': 0.3, + 'min_wrist_yaw_rad': 2.54}, + 'wheel_diameter_m': 0.1016}, + 'collision_arm_camera': { + 'enabled': 1, + 'py_class_name': 'CollisionArmCamera', + 'py_module_name': 'stretch_body.robot_collision_models'}, + 'collision_stretch_gripper': { + 'enabled': 1, + 'py_class_name': 'CollisionStretchGripper', + 'py_module_name': 'stretch_body.robot_collision_models'}, + 'dxl_comm_errors': { + 'warn_every_s': 1.0, + 'warn_above_rate': 0.1, + 'verbose': 0}, + 'end_of_arm':{ + 'usb_name': '/dev/hello-dynamixel-wrist', + 'devices':{ + 'wrist_yaw':{ + 'py_class_name': 'WristYaw', + 'py_module_name': 'stretch_body.wrist_yaw'}}, + 'use_group_sync_read': 1, + 'retry_on_comm_failure': 1, + 'baud': 115200, + 'dxl_latency_timer': 64, + 'stow': {'wrist_yaw': 3.4}}, + 'head':{ + 'usb_name': '/dev/hello-dynamixel-head', + 'use_group_sync_read': 1, + 'retry_on_comm_failure': 1, + 'dxl_latency_timer':64, + 'baud': 115200}, + 'head_pan':{ + 'flip_encoder_polarity': 1, + 'gr': 1.0, + 'id': 11, + 'max_voltage_limit': 15, + 'min_voltage_limit': 11, + 'motion': { + 'trajectory_vel_ctrl':1, + 'trajectory_vel_ctrl_kP':1.5, + 'default': { + 'accel': 8.0, + 'vel': 3.0}, + 'fast': { + 'accel': 10.0, + 'vel': 5.0}, + 'max': { + 'accel': 14.0, + 'vel': 7.0}, + 'slow': { + 'accel': 4.0, + 'vel': 1.0}, + 'trajectory_max': { + 'vel_r': 8.0, + 'accel_r': 16.0}}, + 'pid': [800, 200, 200], + 'pwm_homing': [-300,300], + 'pwm_limit': 885, + 'req_calibration': 0, + 'return_delay_time': 0, + 'temperature_limit': 72, + 'usb_name': '/dev/hello-dynamixel-head', + 'use_multiturn': 0, + 'retry_on_comm_failure': 1, + 'baud': 115200, + 'enable_runstop': 1, + 'disable_torque_on_stop': 1, + 'stall_max_effort': 20.0, + 'stall_backoff': 0.017, + 'stall_max_time': 1.0, + 'stall_min_vel': 0.1, + 'range_pad_t':[50.0,-50.0]}, + 'head_tilt':{ + 'flip_encoder_polarity': 1, + 'gr': 1.0, + 'id': 12, + 'max_voltage_limit': 15, + 'min_voltage_limit': 11, + 'motion': { + 'trajectory_vel_ctrl':1, + 'trajectory_vel_ctrl_kP':1.5, + 'default': { + 'accel': 8.0, + 'vel': 3.0}, + 'fast': { + 'accel': 10.0, + 'vel': 5.0}, + 'max': { + 'accel': 14.0, + 'vel': 7.0}, + 'slow': { + 'accel': 4.0, + 'vel': 1.0}, + 'trajectory_max': { + 'vel_r': 8.0, + 'accel_r': 16.0}}, + 'pid': [800,200,200], + 'pwm_homing': [-300,300], + 'pwm_limit': 885, + 'range_t': [1775,3150], + 'req_calibration': 0, + 'return_delay_time': 0, + 'temperature_limit': 72, + 'usb_name': '/dev/hello-dynamixel-head', + 'use_multiturn': 0, + 'zero_t': 2048, + 'retry_on_comm_failure': 1, + 'baud': 115200, + 'enable_runstop': 1, + 'disable_torque_on_stop': 1, + 'stall_backoff': 0.017, + 'stall_max_effort': 20.0, + 'stall_max_time': 1.0, + 'stall_min_vel': 0.1, + 'range_pad_t': [50.0, -50.0]}, + 'hello-motor-arm':{ + 'gains':{ + 'effort_LPF': 10.0, + 'enable_guarded_mode': 1, + 'enable_runstop': 1, + 'enable_sync_mode': 1, + 'enable_vel_watchdog':0, + 'flip_effort_polarity': 0, + 'flip_encoder_polarity': 0, + 'iMax_neg': -4.35, + 'iMax_pos': 4.35, + 'i_contact_neg': -2.0, + 'i_contact_pos': 2.0, + 'i_safety_feedforward': 0.0, + 'pKd_d': 60.0, + 'pKi_d': 0.1, + 'pKi_limit': 150, + 'pKp_d': 8.0, + 'pLPF': 60, + 'voltage_LPF':1.0, + 'phase_advance_d': 1.8, + 'pos_near_setpoint_d': 2.0, + 'safety_hold': 0, + 'safety_stiffness': 1.0, + 'vKd_d': 0, + 'vKi_d': 0.005, + 'vKi_limit': 200, + 'vKp_d': 0.2, + 'vLPF': 30, + 'vTe_d': 50, + 'vel_near_setpoint_d': 3.5, + 'vel_status_LPF': 10.0}, + 'holding_torque': 1.26, + 'motion':{ + 'accel': 15, + 'vel': 25}, + 'rated_current': 2.8}, + 'hello-motor-left-wheel':{ + 'gains':{ + 'effort_LPF': 2.0, + 'enable_guarded_mode': 0, + 'enable_runstop': 1, + 'enable_sync_mode': 1, + 'enable_vel_watchdog':1, + 'flip_effort_polarity': 1, + 'flip_encoder_polarity': 1, + 'iMax_neg': -4.35, + 'iMax_pos': 4.35, + 'i_contact_neg': -3.0, + 'i_contact_pos': 3.0, + 'i_safety_feedforward': 0.0, + 'pKd_d': 65.0, + 'pKi_d': 0.1, + 'pKi_limit': 200.0, + 'pKp_d': 12.0, + 'pLPF': 80.0, + 'voltage_LPF':1.0, + 'phase_advance_d': 1.8, + 'pos_near_setpoint_d': 1.0, + 'safety_hold': 0, + 'safety_stiffness': 1.0, + 'vKd_d': 0, + 'vKi_d': 0.005, + 'vKi_limit': 200, + 'vKp_d': 0.2, + 'vLPF': 30, + 'vTe_d': 50, + 'vel_near_setpoint_d': 3.5, + 'vel_status_LPF': 10.0}, + 'holding_torque': 1.26, + 'motion':{ + 'accel': 15, + 'vel': 25}, + 'rated_current': 2.8}, + 'hello-motor-lift':{ + 'gains':{ + 'effort_LPF': 2.0, + 'enable_guarded_mode': 1, + 'enable_runstop': 1, + 'enable_sync_mode': 1, + 'enable_vel_watchdog':0, + 'flip_effort_polarity': 1, + 'flip_encoder_polarity': 1, + 'iMax_neg': -4.35, + 'iMax_pos': 4.35, + 'i_contact_neg': -3.0, + 'i_contact_pos': 3.0, + 'i_safety_feedforward': 1.2, + 'pKd_d': 25.0, + 'pKi_d': 0.05, + 'pKi_limit': 100.0, + 'pKp_d': 6.0, + 'pLPF': 100, + 'voltage_LPF':1.0, + 'phase_advance_d': 1.8, + 'pos_near_setpoint_d': 6.0, + 'safety_hold': 1, + 'safety_stiffness': 0.0, + 'vKd_d': 0, + 'vKi_d': 0.005, + 'vKi_limit': 200, + 'vKp_d': 0.2, + 'vLPF': 30, + 'vTe_d': 50, + 'vel_near_setpoint_d': 3.5, + 'vel_status_LPF': 10.0}, + 'holding_torque': 1.9, + 'motion':{ + 'accel': 15, + 'vel': 12}, + 'rated_current': 2.8}, + 'hello-motor-right-wheel':{ + 'gains':{ + 'effort_LPF': 2.0, + 'enable_guarded_mode': 0, + 'enable_runstop': 1, + 'enable_sync_mode': 1, + 'enable_vel_watchdog':1, + 'flip_effort_polarity': 0, + 'flip_encoder_polarity': 0, + 'iMax_neg': -4.35, + 'iMax_pos': 4.35, + 'i_contact_neg': -3.0, + 'i_contact_pos': 3.0, + 'i_safety_feedforward': 0.0, + 'pKd_d': 65.0, + 'pKi_d': 0.1, + 'pKi_limit': 200.0, + 'pKp_d': 12.0, + 'pLPF': 80.0, + 'voltage_LPF':1.0, + 'phase_advance_d': 1.8, + 'pos_near_setpoint_d': 1.0, + 'safety_hold': 0, + 'safety_stiffness': 1.0, + 'vKd_d': 0, + 'vKi_d': 0.005, + 'vKi_limit': 200, + 'vKp_d': 0.2, + 'vLPF': 30, + 'vTe_d': 50, + 'vel_near_setpoint_d': 3.5, + 'vel_status_LPF': 10.0}, + 'holding_torque': 1.26, + 'motion':{ + 'accel': 15, + 'vel': 25}, + 'rated_current': 2.8}, + 'lift':{ + 'usb_name': '/dev/hello-motor-lift', + 'force_N_per_A': 75.0, # Legacy + 'calibration_range_bounds': [1.094, 1.106], + 'contact_models': { + 'effort_pct':{ + 'contact_thresh_calibration_margin': 10.0, + 'contact_thresh_max': [-100, 100]}}, + 'belt_pitch_m': 0.005, + 'motion':{ + 'default':{ + 'accel_m': 0.2, + 'vel_m': 0.11}, + 'fast':{ + 'accel_m': 0.25, + 'vel_m': 0.13}, + 'max':{ + 'accel_m': 0.3, + 'vel_m': 0.15}, + 'slow':{ + 'accel_m': 0.05, + 'vel_m': 0.05}, + 'trajectory_max': { + 'accel_m': 0.3, + 'vel_m': 0.15}}, + 'pinion_t': 12}, + 'pimu':{ + 'usb_name': '/dev/hello-pimu', + 'base_fan_off': 70, + 'base_fan_on': 82, + 'max_sync_rate_hz': 80.0, #deprecated with P3 + 'config':{ + 'accel_LPF': 20.0, + 'bump_thresh': 10.0, + 'cliff_LPF': 10.0, + 'cliff_thresh': -50, + 'current_LPF': 10.0, + 'high_current_alert': 7.0, + 'low_voltage_alert': 10.5, + 'over_tilt_alert': 0.17, + 'stop_at_cliff': 0, + 'stop_at_high_current': 0, + 'stop_at_low_voltage': 1, + 'stop_at_runstop': 1, + 'stop_at_tilt': 0, + 'temp_LPF': 1.0, + 'voltage_LPF': 1.0}}, + 'robot':{ + 'rates':{ + 'DXLStatusThread_Hz': 15.0, + 'NonDXLStatusThread_Hz': 25.0, + 'SystemMonitorThread_Hz': 15.0, + 'SystemMonitorThread_monitor_downrate_int': 2, + 'SystemMonitorThread_trace_downrate_int': 1, + 'SystemMonitorThread_collision_downrate_int': 5, + 'SystemMonitorThread_sentry_downrate_int': 1, + 'SystemMonitorThread_nondxl_trajectory_downrate_int': 2}, + 'tool': 'tool_stretch_gripper', + 'use_collision_manager': 0, + 'stow':{ + 'arm': 0.0, + 'head_pan': 0.0, + 'head_tilt': 0.0, + 'lift': 0.23, + 'stretch_gripper': 0, + 'wrist_yaw': 3.4}, + 'use_monitor': 1, + 'use_trace': 0, + 'use_sentry': 1, + 'use_asyncio':1}, + 'robot_collision': { + 'models': ['collision_arm_camera'] + }, + 'robot_monitor':{ + 'monitor_base_bump_event': 1, + 'monitor_base_cliff_event': 1, + 'monitor_current': 1, + 'monitor_dynamixel_flags': 1, + 'monitor_guarded_contact': 1, + 'monitor_over_tilt_alert': 1, + 'monitor_runstop': 1, + 'monitor_voltage': 1, + 'monitor_wrist_single_tap': 1}, + 'robot_sentry':{ + 'base_fan_control': 1, + 'base_max_velocity': 1, + 'dynamixel_stop_on_runstop': 1, + 'stretch_gripper_overload': 1, + 'wrist_yaw_overload': 1, + 'stepper_is_moving_filter': 1}, + 'robot_trace':{ + 'n_samples_per_file':100, + 'duration_limit_minutes':10.0 + }, + 'stretch_gripper':{ + 'range_pad_t': [100.0, -100.0], + 'flip_encoder_polarity': 0, + 'gr': 1.0, + 'id': 14, + 'max_voltage_limit': 15, + 'min_grip_strength': -125, + 'min_voltage_limit': 11, + 'motion':{ + 'trajectory_vel_ctrl':1, + 'trajectory_vel_ctrl_kP':1.5, + 'default':{ + 'accel': 10.0, + 'vel': 4.0}, + 'fast':{ + 'accel': 10.0, + 'vel': 6.0}, + 'max':{ + 'accel': 12, + 'vel': 8}, + 'slow':{ + 'accel': 4.0, + 'vel': 2.0}, + 'trajectory_max': { + 'vel_r': 50.0, + 'accel_r': 100.0}}, + 'pid': [640.0,0,0], + 'pwm_homing': [-400, 0], + 'pwm_limit': 885, + 'req_calibration': 1, + 'return_delay_time': 0, + 'stall_backoff': 2.0, + 'stall_max_effort': 20.0, + 'stall_max_time': 0.5, + 'stall_min_vel': 0.1, + 'temperature_limit': 72, + 'usb_name': '/dev/hello-dynamixel-wrist', + 'use_multiturn': 1, + 'retry_on_comm_failure': 1, + 'baud': 115200, + 'enable_runstop': 1, + 'disable_torque_on_stop': 1}, + 'tool_none': { + 'use_group_sync_read': 1, + 'retry_on_comm_failure': 1, + 'baud':115200, + 'dxl_latency_timer': 64, + 'py_class_name': 'ToolNone', + 'py_module_name': 'stretch_body.end_of_arm_tools', + 'stow': {'wrist_yaw': 3.4}, + 'devices': { + 'wrist_yaw': { + 'py_class_name': 'WristYaw', + 'py_module_name': 'stretch_body.wrist_yaw'}}}, + 'tool_stretch_gripper': { + 'use_group_sync_read': 1, + 'retry_on_comm_failure': 1, + 'baud':115200, + 'dxl_latency_timer': 64, + 'py_class_name': 'ToolStretchGripper', + 'py_module_name': 'stretch_body.end_of_arm_tools', + 'stow': {'stretch_gripper': 0, 'wrist_yaw': 3.4}, + 'devices': { + 'stretch_gripper': { + 'py_class_name': 'StretchGripper', + 'py_module_name': 'stretch_body.stretch_gripper' + }, + 'wrist_yaw': { + 'py_class_name': 'WristYaw', + 'py_module_name': 'stretch_body.wrist_yaw' + } + }, + 'collision_models': ['collision_stretch_gripper']}, + 'wacc':{ + 'usb_name': '/dev/hello-wacc', + 'config': { + 'accel_LPF': 10.0, + 'accel_range_g': 4, + 'accel_single_tap_dur': 70, + 'accel_single_tap_thresh': 50, + 'ana_LPF': 10.0}}, + 'wrist_yaw':{ + 'flip_encoder_polarity': 1, + 'gr': 2.4, + 'id': 13, + 'max_voltage_limit': 15, + 'min_voltage_limit': 11, + 'motion':{ + 'trajectory_vel_ctrl':1, + 'trajectory_vel_ctrl_kP':1.5, + 'default':{ + 'accel': 3.0, + 'vel': 2.0}, + 'fast':{ + 'accel': 5.0, + 'vel': 2.5}, + 'max':{ + 'accel': 10, + 'vel': 6.0}, + 'slow':{ + 'accel': 1.5, + 'vel': 0.75}, + 'trajectory_max': { + 'vel_r': 3.0, + 'accel_r': 3.0}}, + 'pid': [640,0,0], + 'pwm_homing': [-300,300], + 'pwm_limit': 885, + 'req_calibration': 1, + 'return_delay_time': 0, + 'stall_backoff': 0.017, + 'stall_max_effort': 20.0, + 'stall_max_time': 1.0, + 'stall_min_vel': 0.1, + 'temperature_limit': 72, + 'usb_name': '/dev/hello-dynamixel-wrist', + 'use_multiturn': 1, + 'retry_on_comm_failure': 1, + 'baud': 115200, + 'enable_runstop': 1, + 'disable_torque_on_stop': 1, + 'range_pad_t': [100.0, -100.0]}, + 'respeaker': {'usb_name': '/dev/hello-respeaker'}, + 'lidar': {'usb_name': '/dev/hello-lrf'} +} \ No newline at end of file diff --git a/body/stretch_body/stepper.py b/body/stretch_body/stepper.py index dcb8600b..c32aba39 100644 --- a/body/stretch_body/stepper.py +++ b/body/stretch_body/stepper.py @@ -125,7 +125,8 @@ def __init__(self, usb,name=None): 'transport': self.transport.status,'pos_calibrated':0,'runstop_on':0,'near_pos_setpoint':0,'near_vel_setpoint':0, 'is_moving':0,'is_moving_filtered':0,'at_current_limit':0,'is_mg_accelerating':0,'is_mg_moving':0,'calibration_rcvd': 0,'in_guarded_event':0, 'in_safety_event':0,'waiting_on_sync':0,'in_sync_mode':0,'trace_on':0,'ctrl_cycle_cnt':0, - 'waypoint_traj':{'state':'idle','setpoint':None, 'segment_id':0,}} + 'waypoint_traj':{'state':'idle','setpoint':None, 'segment_id':0,}, + 'voltage':0} self.status_aux={'cmd_cnt_rpc':0,'cmd_cnt_exec':0,'cmd_rpc_overflow':0,'sync_irq_cnt':0,'sync_irq_overflow':0} @@ -274,7 +275,7 @@ def pull_load_test(self): raise NotImplementedError('This method not supported for firmware on protocol {0}.' .format(self.board_info['protocol_version'])) - def pretty_print(self): + def pretty_print(self): #Base raise NotImplementedError('This method not supported for firmware on protocol {0}.' .format(self.board_info['protocol_version'])) @@ -657,11 +658,11 @@ def unpack_board_info(self,s): sidx += 20 return sidx - def unpack_status(self,s): + def unpack_status(self,s,unpack_to=None): #Base raise NotImplementedError('This method not supported for firmware on protocol {0}.' .format(self.board_info['protocol_version'])) - def unpack_gains(self,s): + def unpack_gains(self,s): #Base sidx=0 self.gains_flash['pKp_d'] = unpack_float_t(s[sidx:]);sidx+=4 self.gains_flash['pKi_d'] = unpack_float_t(s[sidx:]);sidx += 4 @@ -721,7 +722,7 @@ def pack_command(self, s, sidx): sidx += 1 return sidx - def pack_gains(self,s,sidx): + def pack_gains(self,s,sidx): #Base pack_float_t(s, sidx, self.gains['pKp_d']);sidx += 4 pack_float_t(s, sidx, self.gains['pKi_d']);sidx += 4 pack_float_t(s, sidx, self.gains['pKd_d']);sidx += 4 @@ -842,35 +843,37 @@ def pull_status_aux(self): # ######################## STEPPER PROTOCOL PO ################################# class Stepper_Protocol_P0(StepperBase): - def unpack_status(self,s): + def unpack_status(self,s,unpack_to=None): #P0 + if unpack_to is None: + unpack_to=self.status sidx=0 - self.status['mode']=unpack_uint8_t(s[sidx:]);sidx+=1 - self.status['effort_ticks'] = unpack_float_t(s[sidx:]);sidx+=4 - self.status['current']=self.effort_ticks_to_current(self.status['effort_ticks']) - self.status['effort_pct']=self.current_to_effort_pct(self.status['current']) - self.status['pos'] = unpack_double_t(s[sidx:]);sidx+=8 - self.status['vel'] = unpack_float_t(s[sidx:]);sidx+=4 - self.status['err'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['diag'] = unpack_uint32_t(s[sidx:]);sidx += 4 - self.status['timestamp'] = self.timestamp.set(unpack_uint32_t(s[sidx:]));sidx += 4 - self.status['debug'] = unpack_float_t(s[sidx:]);sidx += 4 - self.status['guarded_event'] = unpack_uint32_t(s[sidx:]);sidx += 4 - - self.status['pos_calibrated'] =self.status['diag'] & self.DIAG_POS_CALIBRATED > 0 - self.status['runstop_on'] =self.status['diag'] & self.DIAG_RUNSTOP_ON > 0 - self.status['near_pos_setpoint'] =self.status['diag'] & self.DIAG_NEAR_POS_SETPOINT > 0 - self.status['near_vel_setpoint'] = self.status['diag'] & self.DIAG_NEAR_VEL_SETPOINT > 0 - self.status['is_moving'] =self.status['diag'] & self.DIAG_IS_MOVING > 0 - self.status['at_current_limit'] =self.status['diag'] & self.DIAG_AT_CURRENT_LIMIT > 0 - self.status['is_mg_accelerating'] = self.status['diag'] & self.DIAG_IS_MG_ACCELERATING > 0 - self.status['is_mg_moving'] =self.status['diag'] & self.DIAG_IS_MG_MOVING > 0 - self.status['calibration_rcvd'] = self.status['diag'] & self.DIAG_CALIBRATION_RCVD > 0 - self.status['in_guarded_event'] = self.status['diag'] & self.DIAG_IN_GUARDED_EVENT > 0 - self.status['in_safety_event'] = self.status['diag'] & self.DIAG_IN_SAFETY_EVENT > 0 - self.status['waiting_on_sync'] = self.status['diag'] & self.DIAG_WAITING_ON_SYNC > 0 + unpack_to['mode']=unpack_uint8_t(s[sidx:]);sidx+=1 + unpack_to['effort_ticks'] = unpack_float_t(s[sidx:]);sidx+=4 + unpack_to['current']=self.effort_ticks_to_current(unpack_to['effort_ticks']) + unpack_to['effort_pct']=self.current_to_effort_pct(unpack_to['current']) + unpack_to['pos'] = unpack_double_t(s[sidx:]);sidx+=8 + unpack_to['vel'] = unpack_float_t(s[sidx:]);sidx+=4 + unpack_to['err'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['diag'] = unpack_uint32_t(s[sidx:]);sidx += 4 + unpack_to['timestamp'] = self.timestamp.set(unpack_uint32_t(s[sidx:]));sidx += 4 + unpack_to['debug'] = unpack_float_t(s[sidx:]);sidx += 4 + unpack_to['guarded_event'] = unpack_uint32_t(s[sidx:]);sidx += 4 + + unpack_to['pos_calibrated'] =unpack_to['diag'] & self.DIAG_POS_CALIBRATED > 0 + unpack_to['runstop_on'] =unpack_to['diag'] & self.DIAG_RUNSTOP_ON > 0 + unpack_to['near_pos_setpoint'] =unpack_to['diag'] & self.DIAG_NEAR_POS_SETPOINT > 0 + unpack_to['near_vel_setpoint'] = unpack_to['diag'] & self.DIAG_NEAR_VEL_SETPOINT > 0 + unpack_to['is_moving'] =unpack_to['diag'] & self.DIAG_IS_MOVING > 0 + unpack_to['at_current_limit'] =unpack_to['diag'] & self.DIAG_AT_CURRENT_LIMIT > 0 + unpack_to['is_mg_accelerating'] = unpack_to['diag'] & self.DIAG_IS_MG_ACCELERATING > 0 + unpack_to['is_mg_moving'] =unpack_to['diag'] & self.DIAG_IS_MG_MOVING > 0 + unpack_to['calibration_rcvd'] = unpack_to['diag'] & self.DIAG_CALIBRATION_RCVD > 0 + unpack_to['in_guarded_event'] = unpack_to['diag'] & self.DIAG_IN_GUARDED_EVENT > 0 + unpack_to['in_safety_event'] = unpack_to['diag'] & self.DIAG_IN_SAFETY_EVENT > 0 + unpack_to['waiting_on_sync'] = unpack_to['diag'] & self.DIAG_WAITING_ON_SYNC > 0 return sidx - def pretty_print(self): + def pretty_print(self): #P0 print('-----------') print('Mode', self.MODE_NAMES[self.status['mode']]) print('x_des (rad)', self._command['x_des'], '(deg)',rad_to_deg(self._command['x_des'])) @@ -883,6 +886,8 @@ def pretty_print(self): print('Effort (Ticks)', self.status['effort_ticks']) print('Effort (Pct)',self.status['effort_pct']) print('Current (A)', self.status['current']) + if self.board_info['hardware_id']>=3: + print('Voltage (V)',self.status['voltage']) print('Error (deg)', rad_to_deg(self.status['err'])) print('Debug', self.status['debug']) print('Guarded Events:', self.status['guarded_event']) @@ -908,7 +913,7 @@ def pretty_print(self): # ######################## STEPPER PROTOCOL P1 ################################# class Stepper_Protocol_P1(StepperBase): - def unpack_status(self,s,unpack_to=None): + def unpack_status(self,s,unpack_to=None): #P1 if unpack_to is None: unpack_to=self.status sidx=0 @@ -948,7 +953,7 @@ def unpack_status(self,s,unpack_to=None): return sidx - def pretty_print(self): + def pretty_print(self): #P1 print('-----------') print('Mode', self.MODE_NAMES[self.status['mode']]) print('x_des (rad)', self._command['x_des'], '(deg)',rad_to_deg(self._command['x_des'])) @@ -961,6 +966,8 @@ def pretty_print(self): print('Effort (Ticks)', self.status['effort_ticks']) print('Effort (Pct)', self.status['effort_pct']) print('Current (A)', self.status['current']) + if self.board_info['hardware_id'] >= 3: + print('Voltage (V)', self.status['voltage']) print('Error (deg)', rad_to_deg(self.status['err'])) print('Debug', self.status['debug']) print('Guarded Events:', self.status['guarded_event']) @@ -1152,7 +1159,7 @@ def disable_firmware_trace(self): self._dirty_trigger = True - def unpack_status(self,s,unpack_to=None): + def unpack_status(self,s,unpack_to=None): #P2 if unpack_to is None: unpack_to=self.status sidx=0 @@ -1280,6 +1287,30 @@ def rpc_command_reply(self, reply): else: print('Error RPC_REPLY_COMMAND', reply[0]) + +# ######################## STEPPER PROTOCOL P4 ################################# +class Stepper_Protocol_P4(StepperBase): + def unpack_status(self,s,unpack_to=None): #P4 + if unpack_to is None: + unpack_to=self.status + sidx=0 + sidx=sidx+Stepper_Protocol_P2.unpack_status(self,s,unpack_to) + unpack_to['voltage']=self.get_voltage(unpack_float_t(s[sidx:]));sidx+=4 + return sidx + + def get_voltage(self,raw): + raw_to_V = 20.0/1024 #10bit adc, 0-20V per 0-3.3V reading + return raw*raw_to_V + + def pack_gains(self,s,sidx): + sidx=sidx+Stepper_Protocol_P3.pack_gains(self,s,sidx) + pack_float_t(s, sidx, self.gains['voltage_LPF']);sidx += 4 + return sidx + + def unpack_gains(self,s): + sidx=Stepper_Protocol_P3.unpack_gains(self,s) + self.gains_flash['voltage_LPF'] = unpack_float_t(s[sidx:]);sidx += 4 + return sidx # ######################## STEPPER ################################# class Stepper(StepperBase): """ @@ -1291,7 +1322,8 @@ def __init__(self,usb, name=None): self.supported_protocols = {'p0': (Stepper_Protocol_P0,), 'p1': (Stepper_Protocol_P1,Stepper_Protocol_P0,), 'p2': (Stepper_Protocol_P2,Stepper_Protocol_P1,Stepper_Protocol_P0,), - 'p3': (Stepper_Protocol_P3,Stepper_Protocol_P2,Stepper_Protocol_P1,Stepper_Protocol_P0,)} + 'p3': (Stepper_Protocol_P3,Stepper_Protocol_P2,Stepper_Protocol_P1,Stepper_Protocol_P0,), + 'p4': (Stepper_Protocol_P4,Stepper_Protocol_P3,Stepper_Protocol_P2,Stepper_Protocol_P1,Stepper_Protocol_P0,)} def expand_protocol_methods(self, protocol_class): for attr_name, attr_value in protocol_class.__dict__.items(): diff --git a/tools/bin/stretch_pimu_scope.py b/tools/bin/stretch_pimu_scope.py index eab23c83..28a1ea56 100755 --- a/tools/bin/stretch_pimu_scope.py +++ b/tools/bin/stretch_pimu_scope.py @@ -107,16 +107,15 @@ if args.bump: import stretch_body.scope as scope s = scope.Scope(yrange=[-1,15],title='Bump') - itr=0 + bump_cnt_last = p.status['bump_event_cnt'] try: while True: p.pull_status() time.sleep(0.02) - itr=itr+1 - if itr==50: - itr=0 + if p.status['bump_event_cnt']!=bump_cnt_last: p.trigger_beep() p.push_command() + bump_cnt_last = p.status['bump_event_cnt'] print('Bump',p.status['imu']['bump']) s.step_display(p.status['imu']['bump']) except (SystemExit, KeyboardInterrupt):