Skip to content

Commit

Permalink
Merge pull request #206 from hello-robot/features/prince
Browse files Browse the repository at this point in the history 
Features/prince
  • Loading branch information
@aedsinger
aedsinger committed Oct 7, 2023
2 parents 0e4a671 + 4b28b9b commit b97d443
Show file tree
Hide file tree
Showing 7 changed files with 807 additions and 115 deletions.
191 changes: 116 additions & 75 deletions body/stretch_body/pimu.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 #################################
Expand All @@ -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):
Expand Down Expand Up @@ -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}
Expand Down Expand Up @@ -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'])
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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']))

Expand Down Expand Up @@ -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 #################################
Expand Down Expand Up @@ -655,8 +674,6 @@ def unpack_status(self, s, unpack_to=None):





# ######################## PIMU PROTOCOL P2 #################################
class Pimu_Protocol_P2(PimuBase):

Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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):


Expand Down Expand Up @@ -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):
"""
Expand All @@ -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):
"""
Expand Down
6 changes: 5 additions & 1 deletion body/stretch_body/robot_params.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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', []):
Expand Down
4 changes: 4 additions & 0 deletions body/stretch_body/robot_params_RE1V0.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down
4 changes: 4 additions & 0 deletions body/stretch_body/robot_params_RE2V0.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down
Loading

0 comments on commit b97d443

Please sign in to comment.