def initialize_joints(self, right_arm_settings, left_arm_settings): self.proxy = m3p.M3RtProxy() self.proxy.start(True, True) # the second true enables ROS (needed for the skin patch) for c in ['m3pwr_pwr003','m3loadx6_ma1_l0','m3arm_ma1','m3loadx6_ma2_l0','m3arm_ma2']: if not self.proxy.is_component_available(c): raise m3t.M3Exception('Component '+c+' is not available.') self.joint_list_dict = {} right_l = [] for c in ['m3joint_ma1_j0','m3joint_ma1_j1','m3joint_ma1_j2', 'm3joint_ma1_j3','m3joint_ma1_j4','m3joint_ma1_j5', 'm3joint_ma1_j6']: if not self.proxy.is_component_available(c): raise m3t.M3Exception('Component '+c+' is not available.') right_l.append(m3f.create_component(c)) self.joint_list_dict['right_arm'] = right_l left_l = [] for c in ['m3joint_ma2_j0','m3joint_ma2_j1','m3joint_ma2_j2', 'm3joint_ma2_j3','m3joint_ma2_j4','m3joint_ma2_j5', 'm3joint_ma2_j6']: if not self.proxy.is_component_available(c): raise m3t.M3Exception('Component '+c+' is not available.') left_l.append(m3f.create_component(c)) self.joint_list_dict['left_arm'] = left_l for arm,arm_settings in zip(['right_arm','left_arm'],[right_arm_settings,left_arm_settings]): if arm_settings == None: continue for comp in self.joint_list_dict[arm]: self.proxy.subscribe_status(comp) self.proxy.publish_command(comp) self.set_arm_settings(right_arm_settings,left_arm_settings) right_fts=m3.loadx6.M3LoadX6('m3loadx6_ma1_l0') self.proxy.subscribe_status(right_fts) left_fts=m3.loadx6.M3LoadX6('m3loadx6_ma2_l0') self.proxy.subscribe_status(left_fts) self.fts = {'right_arm':right_fts,'left_arm':left_fts} #self.pwr=m3w.M3Pwr('m3pwr_pwr003') self.pwr=m3f.create_component('m3pwr_pwr003') self.proxy.subscribe_status(self.pwr) self.proxy.publish_command(self.pwr) self.arms = {} self.arms['right_arm']=m3.arm.M3Arm('m3arm_ma1') self.proxy.subscribe_status(self.arms['right_arm']) self.arms['left_arm']=m3.arm.M3Arm('m3arm_ma2') self.proxy.subscribe_status(self.arms['left_arm']) self.proxy.step() self.proxy.step()
def calibrate(self, proxy): """ Calibrates Omnibase casters if necessary. :param proxy: running proxy :type proxy: M3RtProxy """ need_to_calibrate = False for i in range(self.num_casters): if (not self.is_calibrated(i)): need_to_calibrate = True if need_to_calibrate: print '------------------------------------------------' print 'All casters not calibrated. Do calibration [y]?' if m3t.get_yes_no('y'): print 'Note: Orientations are facing robot' print "Turn power on to robot and press any key." raw_input() self.set_mode_caster() proxy.step() time.sleep(4) caster_names = [ 'FrontRight', 'RearRight', 'RearLeft', 'FrontLeft' ] wiggle = [1, 2, 1, 2] last_calib = -1 repeat_calib = 0 while need_to_calibrate: for i in [1, 2, 3, 0]: if (not self.is_calibrated(i)): print '-------------------------------------------' print 'Calibrating caster: ', caster_names[i], '..' #print 'Manual assist required in CCW direction' if i == last_calib: repeat_calib += 1 if repeat_calib == 0: wiggle = [1, 2, 1, 2] self.home(i, proxy, wiggle[i]) elif repeat_calib == 1: wiggle = [3, 0, 3, 0] self.home(i, proxy, wiggle[i]) elif repeat_calib == 2: wiggle = [2, 3, 0, 1] self.home(i, proxy, wiggle[i]) elif repeat_calib >= 3: raise m3t.M3Exception( 'Error calibrating. Please reposition base and try again.' ) last_calib = i need_to_calibrate = False for i in range(self.num_casters): if (not self.is_calibrated(i)): need_to_calibrate = True self.set_mode_caster_off(range(self.num_casters)) self.set_mode_off() else: print "Skipping Calibration.."
def __init__(self, q_desired, qdot_avg): self.q_desired = q_desired self.qdot_avg = qdot_avg for i in range(len(qdot_avg)): if qdot_avg[i] <= 0: raise m3t.M3Exception('qdot_avg must be greater than zero')