def start(self):
		self.proxy.start()
		names=self.proxy.get_available_components('m3loadx6_ec')
		if len(names):
			ec_name=m3t.user_select_components_interactive(names)[0]
			name=ec_name.replace('m3loadx6_ec','m3loadx6')
			self.comp_ec=cf.create_component(ec_name)
			self.proxy.subscribe_status(self.comp_ec)
			self.proxy.make_operational(ec_name)
		else:
			print 'No loadx6_ec component available'
			return
		if self.proxy.is_component_available(name):
			self.comp_rt=cf.create_component(name)
			self.proxy.subscribe_status(self.comp_rt)
			self.proxy.make_operational(name)
		else:
			self.comp_rt=None
		#Create gui
		self.fx=[0]
		self.fy=[0]
		self.fz=[0]
		self.tx=[0]
		self.ty=[0]
		self.tz=[0]
		self.status_dict=self.proxy.get_status_dict()
		self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=1)
		self.gui.add('M3GuiSliders','Fx (g)',  (self,'fx'),range(1),[-7500,7500],m3g.M3GuiRead)
		self.gui.add('M3GuiSliders','Fy (g)',  (self,'fy'),range(1),[-7500,7500],m3g.M3GuiRead)
		self.gui.add('M3GuiSliders','Fz (g)',  (self,'fz'),range(1),[-7500,7500],m3g.M3GuiRead)
		self.gui.add('M3GuiSliders','Tx (mNm)',  (self,'tx'),range(1),[-7500,7500],m3g.M3GuiRead)
		self.gui.add('M3GuiSliders','Ty (mNm)',  (self,'ty'),range(1),[-7500,7500],m3g.M3GuiRead)
		self.gui.add('M3GuiSliders','Tz (mNm)',  (self,'tz'),range(1),[-7500,7500],m3g.M3GuiRead)
		self.gui.start(self.step)
Exemple #2
0
def do_log(proxy):
    print 'Starting log...select component to log'
    proxy.start(start_data_svc=False)
    comp_name = m3t.user_select_components_interactive(
        proxy.get_available_components())[0]
    comp = mcf.create_component(comp_name)
    proxy.register_log_component(comp)
    print 'Enter log name [foo]'
    try_again = True
    while try_again:
        logname = m3t.get_string('foo')
        if not os.path.isdir(m3t.get_m3_log_path() + logname):
            try_again = False
        else:
            print 'Log directory already exists.  Please enter another name.'
    print 'Enter sample frequence (0-X Hz) [250.0]'
    freq = m3t.get_float(250.0)
    default_samps = int(2 * freq / 5)
    print 'Enter samples per file [' + str(default_samps) + ']'
    samples = m3t.get_int(default_samps)
    print 'Enter sample time (s) [5.0]'
    duration = m3t.get_float(5.0)
    print 'Hit enter to begin log...'
    raw_input()
    proxy.start_log_service(logname,
                            sample_freq_hz=freq,
                            samples_per_file=samples)
    ts = time.time()
    while time.time() - ts < duration:
        time.sleep(0.25)
        print 'Logging ', logname, ' Time: ', time.time() - ts
    proxy.stop_log_service()
	def start(self,ctype):
		#Return true if success
		#ctype. e.g.,: {'comp_ec':'m3actuator_ec','comp_rt':'m3actuator'}
		self.proxy=m3p.M3RtProxy()
		self.proxy.start()
		print 'Select component'
		ac=self.proxy.get_available_components(ctype['comp_rt'])
		if len(ac)==0:
			print 'Required components not available'
			return False
		self.name_rt=m3t.user_select_components_interactive(ac,single=True)[0]
		self.name_ec=self.name_rt.replace(ctype['comp_rt'],ctype['comp_ec'])
		self.comp_ec=m3f.create_component(self.name_ec)
		self.comp_rt=m3f.create_component(self.name_rt)
		if self.proxy.is_component_available(self.name_ec):
			self.proxy.subscribe_status(self.comp_ec)
			self.proxy.publish_command(self.comp_ec)
			self.proxy.publish_param(self.comp_ec)
			self.proxy.make_operational(self.name_ec)
		if self.proxy.is_component_available(self.name_rt):
			self.proxy.subscribe_status(self.comp_rt)
		pwr_ec=self.proxy.get_available_components('m3pwr_ec')
		pwr_rt=self.proxy.get_available_components('m3pwr')
		if len(pwr_rt):
			pr=m3f.create_component(pwr_rt[0])
			self.proxy.publish_command(pr)
			self.proxy.make_operational(pwr_rt[0])
			pr.set_motor_power_on()
		if len(pwr_ec):
			self.proxy.make_operational(pwr_ec[0])
		self.step()
		return True
Exemple #4
0
 def start(self, ctype):
     #Return true if success
     #ctype. e.g.,: {'comp_ec':'m3actuator_ec','comp_rt':'m3actuator'}
     self.proxy = m3p.M3RtProxy()
     self.proxy.start()
     print 'Select component'
     ac = self.proxy.get_available_components(ctype['comp_rt'])
     if len(ac) == 0:
         print 'Required components not available'
         return False
     self.name_rt = m3t.user_select_components_interactive(ac,
                                                           single=True)[0]
     self.name_ec = self.name_rt.replace(ctype['comp_rt'], ctype['comp_ec'])
     self.comp_ec = m3f.create_component(self.name_ec)
     self.comp_rt = m3f.create_component(self.name_rt)
     if self.proxy.is_component_available(self.name_ec):
         self.proxy.subscribe_status(self.comp_ec)
         self.proxy.publish_command(self.comp_ec)
         self.proxy.publish_param(self.comp_ec)
         self.proxy.make_operational(self.name_ec)
     if self.proxy.is_component_available(self.name_rt):
         self.proxy.subscribe_status(self.comp_rt)
     pwr_ec = self.proxy.get_available_components('m3pwr_ec')
     pwr_rt = self.proxy.get_available_components('m3pwr')
     if len(pwr_rt):
         pr = m3f.create_component(pwr_rt[0])
         self.proxy.publish_command(pr)
         self.proxy.make_operational(pwr_rt[0])
         pr.set_motor_power_on()
     if len(pwr_ec):
         self.proxy.make_operational(pwr_ec[0])
     self.step()
     return True
def do_log(proxy):
       print 'Starting log...select component to log'
       proxy.start(start_data_svc=False)
       comp_name=m3t.user_select_components_interactive(proxy.get_available_components())[0]
       comp=mcf.create_component(comp_name)
       proxy.register_log_component(comp)
       print 'Enter log name [foo]'       
       try_again = True
       while try_again:
              logname=m3t.get_string('foo')
              if not os.path.isdir(m3t.get_m3_log_path()+logname):
                     try_again = False
              else:
                     print 'Log directory already exists.  Please enter another name.'
       print 'Enter sample frequence (0-X Hz) [250.0]'
       freq=m3t.get_float(250.0)
       default_samps = int(2*freq/5)
       print 'Enter samples per file ['+str(default_samps) +']'
       samples=m3t.get_int(default_samps)
       print 'Enter sample time (s) [5.0]'
       duration=m3t.get_float(5.0)
       print 'Hit enter to begin log...'
       raw_input()
       proxy.start_log_service(logname,sample_freq_hz=freq,samples_per_file=samples)
       ts=time.time()
       while time.time()-ts<duration:
              time.sleep(0.25)
              print 'Logging ',logname,' Time: ',time.time()-ts
       proxy.stop_log_service()
Exemple #6
0
    def start(self):
        self.proxy.start()

        bot_name = m3t.get_robot_name()
        if bot_name == "":
            print 'Error: no robot components found:', bot_names
            return
        self.bot = m3.humanoid.M3Humanoid(bot_name)
        arm_names = self.bot.get_available_chains()
        arm_names = [x for x in arm_names if x.find('arm') != -1]
        if len(arm_names) == 0:
            print 'No arms found'
            return
        if len(arm_names) == 1:
            self.arm_name = arm_names[0]
        else:
            self.arm_name = m3t.user_select_components_interactive(
                arm_names, single=True)[0]

        # ####### Setup Proxy #############
        self.proxy.subscribe_status(self.bot)
        self.proxy.publish_command(self.bot)
        self.proxy.make_operational_all()
        self.bot.set_motor_power_on()
        self.ndof = self.bot.get_num_dof(self.arm_name)

        self.theta_curr = [0.0] * self.ndof

        zlift_shm_names = self.proxy.get_available_components(
            'm3joint_zlift_shm')
        if len(zlift_shm_names) > 0:
            self.proxy.make_safe_operational(zlift_shm_names[0])

        omnibase_shm_names = self.proxy.get_available_components(
            'm3omnibase_shm')
        if len(omnibase_shm_names) > 0:
            self.proxy.make_safe_operational(omnibase_shm_names[0])

        humanoid_shm_names = self.proxy.get_available_components(
            'm3humanoid_shm')
        if len(humanoid_shm_names) > 0:
            self.proxy.make_safe_operational(humanoid_shm_names[0])

        print 'WARNING: Before raising E-Stop move a slider to initialize positions.'

        #Create gui
        self.mode = [0]
        self.theta_desire_a = [0] * self.num_dof

        self.status_dict = self.proxy.get_status_dict()
        self.param_dict = self.proxy.get_param_dict()
        #        self.gui.add('M3GuiModes',  'Mode',      (self,'mode'),range(1),[['Off','Current','Torque','Torque_GC','Theta','Theta_IMP','Jnt_Theta','No Brake'],1],m3g.M3GuiWrite)
        self.gui.add('M3GuiTree',
                     'Status', (self, 'status_dict'), [], [],
                     m3g.M3GuiRead,
                     column=1)
        #        self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=1)

        self.gui.start(self.step)
Exemple #7
0
	def start(self):
		self.proxy.start()
		cnames=self.proxy.get_available_components('m3actuator_ec')
		pwr_ec=self.proxy.get_available_components('m3pwr_ec')
		pwr_rt=self.proxy.get_available_components('m3pwr')
		
		print 'Select two ACTUATOR_EC components'
		self.names=m3t.user_select_components_interactive(cnames)
		if len(self.names)!=2:
			print 'Incorrect selection'
			return
		self.actuator_ec=[]
		for name in self.names:
			self.actuator_ec.append(m3f.create_component(name))
			self.proxy.subscribe_status(self.actuator_ec[-1])
			self.proxy.publish_command(self.actuator_ec[-1]) 
			self.proxy.publish_param(self.actuator_ec[-1]) 
			self.proxy.make_operational(name)

		if len(pwr_rt):
			pr=m3f.create_component(pwr_rt[0])
			self.proxy.publish_command(pr)
			self.proxy.make_operational(pwr_rt[0])
			self.proxy.make_operational(pwr_ec[0])
			pr.set_motor_power_on()
		
		tmax=[x.param.t_max for x in self.actuator_ec]
		tmin=[x.param.t_min for x in self.actuator_ec]
		
		
		self.proxy.step()
		for c in self.actuator_ec:
			self.bias.append(c.status.adc_torque)
		tl=min(tmin)-self.bias[0]
		tu=max(tmax)-self.bias[0]
		
		self.cycle=False
		self.cycle_last=False
		self.step_period=[2000.0]*len(self.actuator_ec)
	
		#Create gui
		self.mode=[0]*len(self.actuator_ec)
		self.t_desire=[0]*len(self.actuator_ec)
		self.pwm_desire_a=[0]*len(self.actuator_ec)
		self.pwm_desire_b=[0]*len(self.actuator_ec)
		self.save=False
		self.save_last=False
		self.status_dict=self.proxy.get_status_dict()
		self.param_dict=self.proxy.get_param_dict()
		self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
		self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
		self.gui.add('M3GuiModes',  'Mode',      (self,'mode'),range(len(self.actuator_ec)),[['Off','Pwm','PID'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','tqDesire',  (self,'t_desire'),range(len(self.actuator_ec)),[tl,tu],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','pwmDesireA', (self,'pwm_desire_a'),range(len(self.actuator_ec)),[-3200,3200],m3g.M3GuiWrite) 
		self.gui.add('M3GuiSliders','pwmDesireB', (self,'pwm_desire_b'),range(len(self.actuator_ec)),[-3200,3200],m3g.M3GuiWrite) 
		self.gui.add('M3GuiSliders','StepPeriod (ms) ', (self,'step_period'),range(len(self.actuator_ec)),[0,4000],m3g.M3GuiWrite) 
		self.gui.add('M3GuiToggle', 'Cycle',      (self,'cycle'),[],[['On','Off']],m3g.M3GuiWrite)	
		self.gui.add('M3GuiToggle', 'Save',      (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.start(self.step)
	def get_component(self,name):
		if name[-3:-1] == '_j':
			self.comp_name = name
		else:
			cnames = self.proxy.get_available_components(name)
			if len(cnames)==0:
				print 'No ' + name + ' components found. Exiting...'
				exit()	
			self.comp_name = m3t.user_select_components_interactive(cnames,single=True)[0]
			
		comp_type = [k for k, v in self.comps.iteritems() if self.comp_name.startswith(v['name'])][0]
		self.get_children_components(self.comp_name, comp_type)
 def start(self):
     self.ts=time.time()
     self.proxy.start()
     chain_names=self.proxy.get_available_components('m3ledx2_ec')
     led_name=m3t.user_select_components_interactive(chain_names,single=True)[0]
     pwr_name=self.proxy.get_available_components('m3pwr')[0]
     pwr_ec_name=self.proxy.get_available_components('m3pwr_ec')[0]
     self.pwr=m3f.create_component(pwr_name)
     self.proxy.publish_command(self.pwr)
     self.led=m3f.create_component(led_name)
     self.proxy.publish_command(self.led)
     self.proxy.subscribe_status(self.led)
     self.proxy.make_operational(led_name)
     self.proxy.make_operational(pwr_name)
     self.proxy.make_operational(pwr_ec_name)
     self.proxy.subscribe_status(self.pwr)
     self.pwr.set_motor_power_on()
     self.branch_a_board_a_r=[0]
     self.branch_a_board_a_g=[0]
     self.branch_a_board_a_b=[0]
     self.branch_a_board_b_r=[0]
     self.branch_a_board_b_g=[0]
     self.branch_a_board_b_b=[0]
     self.branch_b_board_a_r=[0]
     self.branch_b_board_a_g=[0]
     self.branch_b_board_a_b=[0]
     self.branch_b_board_b_r=[0]
     self.branch_b_board_b_g=[0]
     self.branch_b_board_b_b=[0]
     self.slew=[10000]
     #Create gui
     self.run=False
     self.run_last=False
     self.enable=False
     self.pulse=False
     self.status_dict=self.proxy.get_status_dict()
     self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=3)
     self.gui.add('M3GuiToggle', 'Enable', (self,'enable'),[],[['On','Off']],m3g.M3GuiWrite,column=1)
     self.gui.add('M3GuiToggle', 'Pulse', (self,'pulse'),[],[['On','Off']],m3g.M3GuiWrite,column=1)
     self.gui.add('M3GuiSliders','Slew', (self,'slew'),range(1),[0,25500],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchA.BoardA.R', (self,'branch_a_board_a_r'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchA.BoardA.G', (self,'branch_a_board_a_g'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchA.BoardA.B', (self,'branch_a_board_a_b'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchA.BoardB.R', (self,'branch_a_board_b_r'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchA.BoardB.G', (self,'branch_a_board_b_g'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchA.BoardB.B', (self,'branch_a_board_b_b'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchB.BoardA.R', (self,'branch_b_board_a_r'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchB.BoardA.G', (self,'branch_b_board_a_g'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchB.BoardA.B', (self,'branch_b_board_a_b'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchB.BoardB.R', (self,'branch_b_board_b_r'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchB.BoardB.G', (self,'branch_b_board_b_g'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.add('M3GuiSliders','BranchB.BoardB.B', (self,'branch_b_board_b_b'),range(1),[0,1023],m3g.M3GuiWrite,column=1) 
     self.gui.start(self.step)
    def start(self):
        self.proxy.start()

        bot_name = m3t.get_robot_name()
        if bot_name == "":
            print "Error: no robot components found:", bot_names
            return
        self.bot = m3.humanoid.M3Humanoid(bot_name)
        arm_names = self.bot.get_available_chains()
        arm_names = [x for x in arm_names if x.find("arm") != -1]
        if len(arm_names) == 0:
            print "No arms found"
            return
        if len(arm_names) == 1:
            self.arm_name = arm_names[0]
        else:
            self.arm_name = m3t.user_select_components_interactive(arm_names, single=True)[0]

        # ####### Setup Proxy #############
        self.proxy.subscribe_status(self.bot)
        self.proxy.publish_command(self.bot)
        self.proxy.make_operational_all()
        self.bot.set_motor_power_on()
        self.ndof = self.bot.get_num_dof(self.arm_name)

        self.theta_curr = [0.0] * self.ndof

        zlift_shm_names = self.proxy.get_available_components("m3joint_zlift_shm")
        if len(zlift_shm_names) > 0:
            self.proxy.make_safe_operational(zlift_shm_names[0])

        omnibase_shm_names = self.proxy.get_available_components("m3omnibase_shm")
        if len(omnibase_shm_names) > 0:
            self.proxy.make_safe_operational(omnibase_shm_names[0])

        humanoid_shm_names = self.proxy.get_available_components("m3humanoid_shm")
        if len(humanoid_shm_names) > 0:
            self.proxy.make_safe_operational(humanoid_shm_names[0])

        print "WARNING: Before raising E-Stop move a slider to initialize positions."

        # Create gui
        self.mode = [0]
        self.theta_desire_a = [0] * self.num_dof

        self.status_dict = self.proxy.get_status_dict()
        self.param_dict = self.proxy.get_param_dict()
        #        self.gui.add('M3GuiModes',  'Mode',      (self,'mode'),range(1),[['Off','Current','Torque','Torque_GC','Theta','Theta_IMP','Jnt_Theta','No Brake'],1],m3g.M3GuiWrite)
        self.gui.add("M3GuiTree", "Status", (self, "status_dict"), [], [], m3g.M3GuiRead, column=1)
        #        self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=1)

        self.gui.start(self.step)
Exemple #11
0
    def get_component(self, name):
        if name[-3:-1] == '_j':
            self.comp_name = name
        else:
            cnames = self.proxy.get_available_components(name)
            if len(cnames) == 0:
                print 'No ' + name + ' components found. Exiting...'
                exit()
            self.comp_name = m3t.user_select_components_interactive(
                cnames, single=True)[0]

        comp_type = [
            k for k, v in self.comps.iteritems()
            if self.comp_name.startswith(v['name'])
        ][0]
        self.get_children_components(self.comp_name, comp_type)
	def start(self,process_cb):
		self.proxy = m3p.M3RtProxy()
		self.proxy.start()
		cnames=self.proxy.get_available_components('m3tactile_pps22_ec')
		if len(cnames)==0:
			print 'No PPS22 sensor present'
			return
		if len(cnames)>1:
			name=m3t.user_select_components_interactive(cnames)[0]
		else:
			name=cnames[0]
		self.pps=m3f.create_component(name)
		self.proxy.subscribe_status(self.pps)
		print 'Place sensor in unloaded state.Hit return when ready'
		raw_input()
		self.proxy.step()
		self.zero=self.pps.get_taxels()
		self.process_cb=process_cb
		gtk.main()
 def start(self, process_cb):
     self.proxy = m3p.M3RtProxy()
     self.proxy.start()
     cnames = self.proxy.get_available_components('m3tactile_pps22_ec')
     if len(cnames) == 0:
         print 'No PPS22 sensor present'
         return
     if len(cnames) > 1:
         name = m3t.user_select_components_interactive(cnames)[0]
     else:
         name = cnames[0]
     self.pps = m3f.create_component(name)
     self.proxy.subscribe_status(self.pps)
     print 'Place sensor in unloaded state.Hit return when ready'
     raw_input()
     self.proxy.step()
     self.zero = self.pps.get_taxels()
     self.process_cb = process_cb
     gtk.main()
Exemple #14
0
 def start(self):
     self.proxy.start()
     names = self.proxy.get_available_components('m3loadx6_ec')
     if len(names):
         ec_name = m3t.user_select_components_interactive(names)[0]
         name = ec_name.replace('m3loadx6_ec', 'm3loadx6')
         self.comp_ec = cf.create_component(ec_name)
         self.proxy.subscribe_status(self.comp_ec)
         self.proxy.make_operational(ec_name)
     else:
         print 'No loadx6_ec component available'
         return
     if self.proxy.is_component_available(name):
         self.comp_rt = cf.create_component(name)
         self.proxy.subscribe_status(self.comp_rt)
         self.proxy.make_operational(name)
     else:
         self.comp_rt = None
     #Create gui
     self.fx = [0]
     self.fy = [0]
     self.fz = [0]
     self.tx = [0]
     self.ty = [0]
     self.tz = [0]
     self.status_dict = self.proxy.get_status_dict()
     self.gui.add('M3GuiTree',
                  'Status', (self, 'status_dict'), [], [],
                  m3g.M3GuiRead,
                  column=1)
     self.gui.add('M3GuiSliders', 'Fx (g)', (self, 'fx'), range(1),
                  [-7500, 7500], m3g.M3GuiRead)
     self.gui.add('M3GuiSliders', 'Fy (g)', (self, 'fy'), range(1),
                  [-7500, 7500], m3g.M3GuiRead)
     self.gui.add('M3GuiSliders', 'Fz (g)', (self, 'fz'), range(1),
                  [-7500, 7500], m3g.M3GuiRead)
     self.gui.add('M3GuiSliders', 'Tx (mNm)', (self, 'tx'), range(1),
                  [-7500, 7500], m3g.M3GuiRead)
     self.gui.add('M3GuiSliders', 'Ty (mNm)', (self, 'ty'), range(1),
                  [-7500, 7500], m3g.M3GuiRead)
     self.gui.add('M3GuiSliders', 'Tz (mNm)', (self, 'tz'), range(1),
                  [-7500, 7500], m3g.M3GuiRead)
     self.gui.start(self.step)
    def start(self):
        self.proxy.start()
        joint_names=self.proxy.get_joint_components()
        if len(joint_names)==0:
            print 'No joint components available'
            self.proxy.stop()
            exit()
        joint_names=m3t.user_select_components_interactive(joint_names)
        actuator_ec_names=[]
        actuator_names=[]
        ctrl_names=[]
        for n in joint_names:
            ctrl = m3t.get_joint_ctrl_component_name(n)
            if ctrl != "":
                ctrl_names.append(ctrl)
            actuator = m3t.get_joint_actuator_component_name(n)
            if actuator != "":
                actuator_names.append(actuator)
                actuator_ec = m3t.get_actuator_ec_component_name(actuator)
                if actuator_ec != "":
                    actuator_ec_names.append(actuator_ec)

       

        self.joint=[]
        self.actuator_ec=[]
        self.actuator=[]
        self.ctrl=[]

        for n in actuator_ec_names:
            c=m3f.create_component(n)
            if c is not None:
                try:
                    self.actuator_ec.append(c)
                    self.proxy.subscribe_status(self.actuator_ec[-1])
                    self.proxy.publish_param(self.actuator_ec[-1]) 
                except:
                    print 'Component',n,'not available'

        for n in actuator_names:
            c=m3f.create_component(n)
            if c is not None:
                self.actuator.append(c)
                self.proxy.subscribe_status(self.actuator[-1])
                self.proxy.publish_param(self.actuator[-1]) 
                
        for n in ctrl_names:
            c=m3f.create_component(n)
            if c is not None:
                self.ctrl.append(c)
                self.proxy.subscribe_status(self.ctrl[-1])
                self.proxy.publish_param(self.ctrl[-1]) 

        for n in joint_names:
            c=m3f.create_component(n)
            if c is not None:
                self.joint.append(c)
                self.proxy.subscribe_status(self.joint[-1])
                self.proxy.publish_command(self.joint[-1])
                self.proxy.publish_param(self.joint[-1]) 

        #Enable motor power
        pwr_rt=m3t.get_actuator_ec_pwr_component_name(actuator_ec_names[0])
        self.pwr=m3f.create_component(pwr_rt)
        if self.pwr is not None:
            self.proxy.publish_command(self.pwr)
            self.pwr.set_motor_power_on()

        #Start them all up
        self.proxy.make_operational_all()

        #Force safe-op of robot, etc are present
        types=['m3humanoid','m3hand','m3gripper']
        for t in types:
            cc=self.proxy.get_available_components(t)
            for ccc in cc:
                self.proxy.make_safe_operational(ccc)

        #Force safe-op of chain so that gravity terms are computed
        self.chain=None    
        if len(joint_names)>0:
            for j in joint_names:
                chain_name=m3t.get_joint_chain_name(j)
                if chain_name!="":
                    self.proxy.make_safe_operational(chain_name)
                    #self.chain=m3f.create_component(chain_name)
                    #self.proxy.publish_param(self.chain) #allow to set payload
                    #Force safe-op of chain so that gravity terms are computed
                    dynamatics_name = m3t.get_chain_dynamatics_component_name(chain_name)
                    if dynamatics_name != "":        
                        self.proxy.make_safe_operational(dynamatics_name)
                        self.dyn=m3f.create_component(dynamatics_name)
                        self.proxy.publish_param(self.dyn) #allow to set payload


        #Force safe-op of robot so that gravity terms are computed
        robot_name = m3t.get_robot_name()
        if robot_name != "":
            try:
                self.proxy.make_safe_operational(robot_name)
                self.robot=m3f.create_component(robot_name)
                self.proxy.subscribe_status(self.robot)
                self.proxy.publish_param(self.robot) #allow to set payload  
            except:
                print 'Component',robot_name,'not available'  

        tmax=max([x.param.max_tq for x in self.actuator])
        tmin=min([x.param.min_tq for x in self.actuator])

        qmax=max([x.param.max_q for x in self.joint])
        qmin=min([x.param.min_q for x in self.joint])
        
        ## Plots
        self.scope_torque=[]
        self.scope_theta=[]
        self.scope_thetadot=[]
        self.scope_thetadotdot=[]
        self.scope_torquedot=[]
        
        for i,name in zip(xrange(len(joint_names)),joint_names):
            self.scope_torque.append(       m3t.M3ScopeN(xwidth=100,yrange=None,title='Torque')     )
            self.scope_theta.append(        m3t.M3ScopeN(xwidth=100,yrange=None,title='Theta')      )
            self.scope_thetadot.append(     m3t.M3ScopeN(xwidth=100,yrange=None,title='ThetaDot')   )
            self.scope_thetadotdot.append(  m3t.M3ScopeN(xwidth=100,yrange=None,title='ThetaDotDot'))
            self.scope_torquedot.append(    m3t.M3ScopeN(xwidth=100,yrange=None,title='TorqueDot')  )
            
        #Create gui
        self.mode=[0]*len(self.joint)
        self.tq_desire_a=[0]*len(self.joint)
        self.tq_desire_b=[0]*len(self.joint)
        self.pwm_desire=[0]*len(self.joint)
        self.theta_desire_a=[0]*len(self.joint)
        self.theta_desire_b=[0]*len(self.joint)
        self.thetadot_desire=[0]*len(self.joint)
        self.stiffness=[0]*len(self.joint)
        self.slew=[0]*len(self.joint)
        self.step_period=[2000.0]*len(self.joint)
        self.cycle_theta=False
        self.cycle_last_theta=False
        self.cycle_thetadot=False
        self.cycle_last_thetadot=False
        self.cycle_torque=False
        self.cycle_last_torque=False
        self.save=False
        self.do_scope_torque=False
        self.do_scope_torquedot=False
        self.do_scope_theta=False
        self.do_scope_thetadot=False
        self.do_scope_thetadotdot=False
        self.brake=False
        self.save_last=False
        self.status_dict=self.proxy.get_status_dict()
        self.param_dict=self.proxy.get_param_dict()
        self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
        self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
        self.gui.add('M3GuiModes',  'Mode',      (self,'mode'),range(len(self.joint)),[['Off','Pwm','Torque','Theta','Torque_GC','Theta_GC','Theta_MJ', 'Theta_GC_MJ','Pose','Torque_GRAV_MODEL','ThetaDot_GC','ThetaDot'],1],m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders','TorqueA (mNm)',  (self,'tq_desire_a'),range(len(self.joint)),[tmin,tmax],m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders','TorqueB (mNm)',  (self,'tq_desire_b'),range(len(self.joint)),[tmin,tmax],m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders','Pwm', (self,'pwm_desire'),range(len(self.joint)),[-3200,3200],m3g.M3GuiWrite) 
        self.gui.add('M3GuiSliders','Theta A(Deg)', (self,'theta_desire_a'),range(len(self.joint)),[qmin,qmax],m3g.M3GuiWrite,column=2) 
        self.gui.add('M3GuiSliders','Theta B(Deg)', (self,'theta_desire_b'),range(len(self.joint)),[qmin,qmax],m3g.M3GuiWrite,column=2)     
        self.gui.add('M3GuiSliders','Thetadot (Deg)', (self,'thetadot_desire'),range(len(self.joint)),[-120.0,120.0],m3g.M3GuiWrite,column=2)         
        self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.joint)),[0,100],m3g.M3GuiWrite,column=3) 
        self.gui.add('M3GuiSliders','Slew ', (self,'slew'),range(len(self.joint)),[0,100],m3g.M3GuiWrite,column=3) 
        self.gui.add('M3GuiToggle', 'Save',      (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeTorque',      (self,'do_scope_torque'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeTorqueDot',      (self,'do_scope_torquedot'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeTheta',      (self,'do_scope_theta'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeThetaDot',      (self,'do_scope_thetadot'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeThetaDotDot',      (self,'do_scope_thetadotdot'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders','StepPeriod (ms) ', (self,'step_period'),range(len(self.joint)),[0,8000],m3g.M3GuiWrite)     
        self.gui.add('M3GuiToggle', 'CycleTheta',      (self,'cycle_theta'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'CycleThetaDot',      (self,'cycle_thetadot'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'CycleTorque',      (self,'cycle_torque'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'Brake',      (self,'brake'),[],[['On','Off']],m3g.M3GuiWrite)
        self.gui.start(self.step)
	def start(self):
		self.proxy.start()
		
		sea_joint_names=self.proxy.get_joint_components()
		sea_joint_names=m3t.user_select_components_interactive(sea_joint_names)
		
		self.sea_joint=[]
		    
		for n in sea_joint_names:
			self.sea_joint.append(m3f.create_component(n))			
			self.proxy.subscribe_status(self.sea_joint[-1])			
		
		chain_names=self.proxy.get_chain_components()
		self.chain=[]
		if len(chain_names)>0:
			print 'Select kinematic chain'
			chain_names=m3t.user_select_components_interactive(chain_names)

		for n in chain_names:
			self.chain.append(m3f.create_component(n))
			self.proxy.subscribe_status(self.chain[-1])
								
		#Setup Components
		base_name=self.proxy.get_available_components('m3omnibase')
		if len(base_name)!=1:
			print 'Invalid number of base components available'
			self.proxy.stop()
			exit()
		self.omni=m3o.M3OmniBase(base_name[0])
		print 'Base name ='+base_name[0]
		self.proxy.publish_param(self.omni) 
		self.proxy.subscribe_status(self.omni)
		self.proxy.publish_command(self.omni)
		
		pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])]
		print "pwr_name :",pwr_name
		self.pwr=m3f.create_component(pwr_name[0])
		
		self.proxy.subscribe_status(self.pwr)
		self.proxy.publish_command(self.pwr) 
		self.proxy.make_operational(pwr_name[0])
		self.proxy.step()
					
		self.pwr.set_motor_power_on()
		
		self.proxy.make_operational_all()
		
		self.proxy.step()
		
		self.omni.calibrate(self.proxy)

		#Create gui
		self.ctrl_mode=[0]
		self.traj_mode=[0]
		self.joy_button=[0]
		self.local_velocity_desired_x=[0]
		self.local_velocity_desired_y=[0]
		self.local_velocity_desired_heading=[0]
		self.joy_x=[0]
		self.joy_y=[0]
		self.joy_yaw=[0]
		self.goal_x=[0]
		self.goal_y=[0]
		self.goal_yaw=[0]
		self.caster_ctrl_idx=[0]
		self.caster_ctrl_mode=[0]
		self.steer_or_roll_mode=[0]
		self.op_force_desired_x=[0]
		self.op_force_desired_y=[0]
		self.op_torque_desired=[0]
		self.joint_torque=[0]		
		self.joint_vel=[0]
		self.save=False
		self.save_last=False
		self.init_g = self.omni.get_global_position()
		
		self.status_dict=self.proxy.get_status_dict()
		self.param_dict=self.proxy.get_param_dict()
		self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
		self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
		self.gui.add('M3GuiModes',  'CtrlMode',      (self,'ctrl_mode'),range(1),[['Off','Calibrate','Caster','OpSpaceForce','OpSpaceTraj','CartLocal','CartGlob'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'TrajMode',      (self,'traj_mode'),range(1),[['Off','Joystick','Goal','Vias'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'Caster Steer/Roll',      (self,'steer_or_roll_mode'),range(1),[['Steer','Roll'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'CasterCtrlIdx',      (self,'caster_ctrl_idx'),range(1),[['0','1','2','3'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'CasterCtrlMode',      (self,'caster_ctrl_mode'),range(1),[['Off','Tq','Vel'],1],m3g.M3GuiWrite)		
		self.gui.add('M3GuiModes',  'JoyButton',      (self,'joy_button'),range(1),[['Tri','Circ','Sqr','X'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','JoystickX', (self,'joy_x'),range(1),[-1,1],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','JoystickY', (self,'joy_y'),range(1),[-1,1],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','JoystickYaw', (self,'joy_yaw'),range(1),[-1,1],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','LocalVelX (m/s)', (self,'local_velocity_desired_x'),range(1),[-1,1],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','LocalVelY (m/s)', (self,'local_velocity_desired_y'),range(1),[-1,1],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','LocalVelHeading (deg/s)', (self,'local_velocity_desired_heading'),range(1),[-60,60],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','OpForceX (N)', (self,'op_force_desired_x'),range(1),[-1000,1000],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','OpForceY (N)', (self,'op_force_desired_y'),range(1),[-1000,1000],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','OpTorque (Nm)', (self,'op_torque_desired'),range(1),[-50,50],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','Jnt Torque (Nm)', (self,'joint_torque'),range(1),[-12.8,12.8],m3g.M3GuiWrite,column=1)		
#		self.gui.add('M3GuiSliders','Jnt Torque (Nm)', (self,'joint_torque'),range(1),[-6.4,6.4],m3g.M3GuiWrite,column=1)		
		self.gui.add('M3GuiSliders','Jnt Vel (deg/s)', (self,'joint_vel'),range(1),[-90,90],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','Goal X (m)', (self,'goal_x'),range(1),[-2,2],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','Goal Y (m)', (self,'goal_y'),range(1),[-2,2],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','Goal Yaw(deg)', (self,'goal_yaw'),range(1),[-180,180],m3g.M3GuiWrite,column=1)
		time.sleep(0.5)
		self.omni.set_local_position(0,0,0,self.proxy)
		self.omni.set_global_position(0,0,0,self.proxy)
		self.gui.start(self.step)
	def start(self):
		self.proxy.start()
		print '--------------------------'
		print 'Enable RVIZ? (y/n)'
		print '--------------------------'		
		print
		self.rviz = False
		if m3t.get_yes_no():
			self.rviz = True
			
		sea_joint_names=self.proxy.get_joint_components()
		sea_joint_names=m3t.user_select_components_interactive(sea_joint_names)
		
		self.sea_joint=[]
		    
		for n in sea_joint_names:
			self.sea_joint.append(m3f.create_component(n))			
			self.proxy.subscribe_status(self.sea_joint[-1])
		
		chain_names=self.proxy.get_chain_components()
		self.chain=[]
		if len(chain_names)>0:
			print 'Select kinematic chain'
			chain_names=m3t.user_select_components_interactive(chain_names)
			
		
		for n in chain_names:
			self.chain.append(m3f.create_component(n))
			self.proxy.subscribe_status(self.chain[-1])
						
		kine_names=self.proxy.get_available_components('m3dynamatics')
		self.kine = []
		if len(kine_names)>0:
			print 'Select dynamatics controller'
			kine_names=m3t.user_select_components_interactive(kine_names)
			
		for n in kine_names:
			self.kine.append(m3f.create_component(n))			
			self.proxy.subscribe_status(self.kine[-1])
	
		bot_name=m3t.get_robot_name()
		if bot_name == "":
			print 'Error: no robot components found:', bot_names
			return
		self.bot=m3f.create_component(bot_name)
				
		if self.rviz:			
			self.viz = m3v.M3Viz(self.proxy, self.bot)			
		
		#self.proxy.publish_param(self.bot) #allow to set payload
		#self.proxy.subscribe_status(self.bot)
		#self.proxy.publish_command(self.bot)		
		#self.proxy.make_operational_all()
		self.bot.initialize(self.proxy)
		self.chain_names = self.bot.get_available_chains()
		#Create gui
		self.mode=[0]
		
		self.theta_desire_right_arm=[0]*self.bot.get_num_dof('right_arm')
		self.theta_desire_left_arm=[0]*self.bot.get_num_dof('left_arm')
		self.theta_desire_torso=[0]*self.bot.get_num_dof('torso')
		self.theta_desire_head=[0]*self.bot.get_num_dof('head')
		self.stiffness_right_arm=[0]*self.bot.get_num_dof('right_arm')
		self.stiffness_left_arm=[0]*self.bot.get_num_dof('left_arm')
		self.stiffness_torso=[0]*self.bot.get_num_dof('torso')
		self.stiffness_head=[0]*self.bot.get_num_dof('head')
		self.stiffness=[0]
		#self.slew=[0]
		self.save=False
		self.save_last=False
		self.status_dict=self.proxy.get_status_dict()
		self.param_dict=self.proxy.get_param_dict()
		self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=3)
		self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
		self.gui.add('M3GuiModes',  'Mode',      (self,'mode'),range(1),[['Off','Pwm','Torque','Theta','Torque_GC','Theta_GC','Theta_MJ', 'Theta_GC_MJ'],1],m3g.M3GuiWrite)
		#self.gui.add('M3GuiSliders','Torque (mNm)', (self,'tq_desire'),range(len(self.bot.right_arm_ndof)),[-8000,8000],m3g.M3GuiWrite)
		#self.gui.add('M3GuiSliders','Pwm', (self,'pwm_desire'),range(len(self.sea_joint)),[-3200,3200],m3g.M3GuiWrite) 
		#self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(1),[0,100],m3g.M3GuiWrite,column=1) 
		#self.gui.add('M3GuiSliders','Slew ', (self,'slew'),range(0),[0,100],m3g.M3GuiWrite,column=3) 
		self.gui.add('M3GuiSliders','Theta RA (Deg)', (self,'theta_desire_right_arm'),range(len(self.theta_desire_right_arm)),[-45,140],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','Theta LA (Deg)', (self,'theta_desire_left_arm'),range(len(self.theta_desire_left_arm)),[-45,140],m3g.M3GuiWrite,column=1) 
		self.gui.add('M3GuiSliders','Theta T (Deg)', (self,'theta_desire_torso'),range(len(self.theta_desire_torso)),[-45,140],m3g.M3GuiWrite,column=1) 
		self.gui.add('M3GuiSliders','Theta H (Deg)', (self,'theta_desire_head'),range(len(self.theta_desire_head)),[-45,140],m3g.M3GuiWrite,column=1) 
		self.gui.add('M3GuiSliders','Stiffness RA ', (self,'stiffness_right_arm'),range(len(self.stiffness_right_arm)),[0,100],m3g.M3GuiWrite,column=2)
		self.gui.add('M3GuiSliders','Stiffness LA )', (self,'stiffness_left_arm'),range(len(self.stiffness_left_arm)),[0,100],m3g.M3GuiWrite,column=2) 
		self.gui.add('M3GuiSliders','Stiffness T ', (self,'stiffness_torso'),range(len(self.stiffness_torso)),[0,100],m3g.M3GuiWrite,column=2) 
		
		#self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.sea_joint)),[0,100],m3g.M3GuiWrite,column=3) 
		#self.gui.add('M3GuiToggle', 'Save',      (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)

		self.gui.start(self.step)
    def start(self):
        # ######## Setup Proxy and Components #########################
        self.proxy.start()
        bot_name = m3t.get_robot_name()
        if bot_name == "":
            print "Error: no robot components found:", bot_names
            return
        self.bot = m3.humanoid.M3Humanoid(bot_name)
        arm_names = self.bot.get_available_chains()
        arm_names = [x for x in arm_names if x.find("arm") != -1]
        if len(arm_names) == 0:
            print "No arms found"
            return
        if len(arm_names) == 1:
            self.arm_name = arm_names[0]
        else:
            self.arm_name = m3t.user_select_components_interactive(arm_names, single=True)[0]

            # ####### Setup Hand #############
        hand_names = self.proxy.get_available_components("m3hand")
        hand_name = ""
        if len(hand_names) > 1:
            hand_name = m3t.user_select_components_interactive(chain_names, single=True)
        if len(hand_names) == 1:
            hand_name = hand_names[0]
        if len(hand_name):
            self.hand = m3.hand.M3Hand(hand_name)
            self.proxy.publish_command(self.hand)
            self.proxy.subscribe_status(self.hand)
        else:
            self.hand = None
            # ####### Setup Proxy #############
        self.proxy.subscribe_status(self.bot)
        self.proxy.publish_command(self.bot)
        self.proxy.make_operational_all()
        self.bot.set_motor_power_on()
        self.ndof = self.bot.get_num_dof(self.arm_name)
        self.via_traj = {}
        self.via_traj_first = True

        # ######## Demo and GUI #########################
        self.off = False
        self.grasp = False
        self.hammer_up = False
        self.switch_start = True
        self.task_mode_names = ["Off", "Zero", "HammerGrasp", "Hammer"]
        self.arm_mode_methods = [self.step_off, self.step_zero, self.step_hammer_grasp, self.step_hammer]
        self.hand_mode_methods = [self.step_hand_off, self.step_hand_off, self.step_hand_grasp, self.step_hand_grasp]
        self.task_mode = [0]
        self.poses = {
            "zero": {"right_arm": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]},
            "hammer_grasp": {"right_arm": [33, 0, 0, 120.18858337402344, 0, -60, 0]},
            "hammer_up": {"right_arm": [33, 0, 0, 120.18858337402344, 0, -60, 0]},
            "hammer_down": {"right_arm": [33, 0, 0, 71.249755859375, 0, -60, 0]},
        }

        self.stiffness = [100]
        self.velocity = [25]
        self.cycle_time = [3000]
        self.gui.add(
            "M3GuiModes", "TaskMode", (self, "task_mode"), range(1), [self.task_mode_names, 1], m3g.M3GuiWrite, column=1
        )
        if self.hand is not None:
            self.gui.add(
                "M3GuiToggle", "Grasp", (self, "grasp"), [], [["GraspOpen", "GraspClosed"]], m3g.M3GuiWrite, column=1
            )
        self.gui.add("M3GuiSliders", "Stiffness ", (self, "stiffness"), [0], [0, 100], m3g.M3GuiWrite, column=1)
        self.gui.add("M3GuiSliders", "Velocity ", (self, "velocity"), [0], [0, 40], m3g.M3GuiWrite, column=1)
        self.gui.add("M3GuiSliders", "CycleTimeMs", (self, "cycle_time"), [0], [0, 4000], m3g.M3GuiWrite, column=1)
        self.gui.start(self.step)
    def start(self):
        self.proxy.start()
        cnames = self.proxy.get_available_components("m3actuator_ec")
        self.names = m3t.user_select_components_interactive(cnames)
        if len(self.names) == 0:
            return
        self.actuator_ec = []
        for name in self.names:
            self.actuator_ec.append(m3f.create_component(name))
            self.proxy.subscribe_status(self.actuator_ec[-1])
            self.proxy.publish_command(self.actuator_ec[-1])
            self.proxy.publish_param(self.actuator_ec[-1])
            self.proxy.make_operational(name)

            # pwr_ec=self.proxy.get_available_components('m3pwr_ec')
            # pwr_rt=self.proxy.get_available_components('m3pwr')
            # print 'A',pwr_rt[0],pwr_ec[0]
            # if len(pwr_rt):
            # pr=m3f.create_component(pwr_rt[0])
            # self.proxy.publish_command(pr)
            # self.proxy.make_operational(pwr_rt[0])
            # self.proxy.make_operational(pwr_ec[0])
            # pr.set_motor_power_on()

        pwr_rt = m3t.get_actuator_ec_pwr_component_name(self.names[0])
        pwr_ec = pwr_rt.replace("m3pwr", "m3pwr_ec")
        pr = m3f.create_component(pwr_rt)
        self.proxy.publish_command(pr)
        self.proxy.make_operational(pwr_rt)
        self.proxy.make_operational(pwr_ec)
        pr.set_motor_power_on()

        tmax = [x.param.t_max for x in self.actuator_ec]
        tmin = [x.param.t_min for x in self.actuator_ec]

        self.proxy.step()
        for c in self.actuator_ec:
            self.bias.append(c.status.adc_torque)
        tl = min(tmin) - self.bias[0]
        tu = max(tmax) - self.bias[0]

        self.cycle_pwm = False
        self.cycle_last_pwm = False
        self.cycle_tq = False
        self.cycle_last_tq = False
        self.step_period = [2000.0] * len(self.actuator_ec)
        self.brake = [0]
        # Create gui
        self.mode = [0] * len(self.actuator_ec)
        self.t_desire_a = [0] * len(self.actuator_ec)
        self.t_desire_b = [0] * len(self.actuator_ec)
        self.pwm_desire_a = [0] * len(self.actuator_ec)
        self.pwm_desire_b = [0] * len(self.actuator_ec)
        self.current_desire_a = [0] * len(self.actuator_ec)
        self.current_desire_b = [0] * len(self.actuator_ec)
        self.save = False
        self.save_last = False
        self.do_scope_torque = False
        self.scope_torque = None
        self.status_dict = self.proxy.get_status_dict()
        self.param_dict = self.proxy.get_param_dict()
        self.gui.add("M3GuiTree", "Status", (self, "status_dict"), [], [], m3g.M3GuiRead, column=2)
        self.gui.add("M3GuiTree", "Param", (self, "param_dict"), [], [], m3g.M3GuiWrite, column=3)
        self.gui.add(
            "M3GuiModes",
            "Mode",
            (self, "mode"),
            range(len(self.actuator_ec)),
            [["Off", "Pwm", "PID", "CURRENT"], 1],
            m3g.M3GuiWrite,
        )
        self.gui.add("M3GuiModes", "Brake", (self, "brake"), range(1), [["Enabled", "Disabled"], 1], m3g.M3GuiWrite)
        self.gui.add(
            "M3GuiSliders", "tqDesire", (self, "t_desire_a"), range(len(self.actuator_ec)), [tl, tu], m3g.M3GuiWrite
        )
        self.gui.add(
            "M3GuiSliders", "tqDesire", (self, "t_desire_b"), range(len(self.actuator_ec)), [tl, tu], m3g.M3GuiWrite
        )
        self.gui.add(
            "M3GuiSliders",
            "pwmDesireA",
            (self, "pwm_desire_a"),
            range(len(self.actuator_ec)),
            [-3200, 3200],
            m3g.M3GuiWrite,
        )
        self.gui.add(
            "M3GuiSliders",
            "pwmDesireB",
            (self, "pwm_desire_b"),
            range(len(self.actuator_ec)),
            [-3200, 3200],
            m3g.M3GuiWrite,
        )
        self.gui.add(
            "M3GuiSliders",
            "currentDesireA",
            (self, "current_desire_a"),
            range(len(self.actuator_ec)),
            [-100, 100],
            m3g.M3GuiWrite,
        )
        self.gui.add(
            "M3GuiSliders",
            "currentDesireB",
            (self, "current_desire_b"),
            range(len(self.actuator_ec)),
            [-3200, 3200],
            m3g.M3GuiWrite,
        )
        self.gui.add(
            "M3GuiSliders",
            "StepPeriod (ms) ",
            (self, "step_period"),
            range(len(self.actuator_ec)),
            [0, 4000],
            m3g.M3GuiWrite,
        )
        self.gui.add("M3GuiToggle", "CyclePwm", (self, "cycle_pwm"), [], [["On", "Off"]], m3g.M3GuiWrite)
        self.gui.add("M3GuiToggle", "CycleTq", (self, "cycle_tq"), [], [["On", "Off"]], m3g.M3GuiWrite)
        self.gui.add("M3GuiToggle", "Save", (self, "save"), [], [["On", "Off"]], m3g.M3GuiWrite)
        self.gui.add("M3GuiToggle", "Scope", (self, "do_scope_torque"), [], [["On", "Off"]], m3g.M3GuiWrite)
        self.gui.start(self.step)

import time
import m3.rt_proxy as m3p
import m3.toolbox as m3t
import m3.component_factory as m3f
import Numeric as nu
import math

# ######################################################	
proxy = m3p.M3RtProxy()
proxy.start()
proxy.make_operational_all()
chain_names=proxy.get_available_components('m3hand')
if len(chain_names)>1:
    hand_name=m3t.user_select_components_interactive(chain_names,single=True)
else:
    hand_name=chain_names
pwr_name=proxy.get_available_components('m3pwr')[0]
hand=m3f.create_component(hand_name[0])
proxy.publish_command(hand)
proxy.subscribe_status(hand)

pwr=m3f.create_component(pwr_name)
proxy.publish_command(pwr)
pwr.set_motor_power_on()

hand.set_mode_torque()
print 'Enter duration (s) [30.0]'
d=m3t.get_float(30.0)
ts=time.time()
    def start(self):
        self.proxy.start()

        sea_joint_names = self.proxy.get_joint_components()
        sea_joint_names = m3t.user_select_components_interactive(
            sea_joint_names)

        self.sea_joint = []

        for n in sea_joint_names:
            self.sea_joint.append(m3f.create_component(n))
            self.proxy.subscribe_status(self.sea_joint[-1])

        chain_names = self.proxy.get_chain_components()
        self.chain = []
        if len(chain_names) > 0:
            print 'Select kinematic chain'
            chain_names = m3t.user_select_components_interactive(chain_names)

        for n in chain_names:
            self.chain.append(m3f.create_component(n))
            self.proxy.subscribe_status(self.chain[-1])

        #Setup Components
        base_name = self.proxy.get_available_components('m3omnibase')
        if len(base_name) != 1:
            print 'Invalid number of base components available'
            self.proxy.stop()
            exit()
        self.omni = m3o.M3OmniBase(base_name[0])
        print 'Base name =' + base_name[0]
        self.proxy.publish_param(self.omni)
        self.proxy.subscribe_status(self.omni)
        self.proxy.publish_command(self.omni)

        pwr_name = [m3t.get_omnibase_pwr_component_name(base_name[0])]
        print "pwr_name :", pwr_name
        self.pwr = m3f.create_component(pwr_name[0])

        self.proxy.subscribe_status(self.pwr)
        self.proxy.publish_command(self.pwr)
        self.proxy.make_operational(pwr_name[0])
        self.proxy.step()

        self.pwr.set_motor_power_on()

        self.proxy.make_operational_all()

        self.proxy.step()

        self.omni.calibrate(self.proxy)

        #Create gui
        self.ctrl_mode = [0]
        self.traj_mode = [0]
        self.joy_button = [0]
        self.local_velocity_desired_x = [0]
        self.local_velocity_desired_y = [0]
        self.local_velocity_desired_heading = [0]
        self.joy_x = [0]
        self.joy_y = [0]
        self.joy_yaw = [0]
        self.goal_x = [0]
        self.goal_y = [0]
        self.goal_yaw = [0]
        self.caster_ctrl_idx = [0]
        self.caster_ctrl_mode = [0]
        self.steer_or_roll_mode = [0]
        self.op_force_desired_x = [0]
        self.op_force_desired_y = [0]
        self.op_torque_desired = [0]
        self.joint_torque = [0]
        self.joint_vel = [0]
        self.save = False
        self.save_last = False
        self.init_g = self.omni.get_global_position()

        self.status_dict = self.proxy.get_status_dict()
        self.param_dict = self.proxy.get_param_dict()
        self.gui.add('M3GuiTree',
                     'Status', (self, 'status_dict'), [], [],
                     m3g.M3GuiRead,
                     column=2)
        self.gui.add('M3GuiTree',
                     'Param', (self, 'param_dict'), [], [],
                     m3g.M3GuiWrite,
                     column=3)
        self.gui.add('M3GuiModes', 'CtrlMode', (self, 'ctrl_mode'), range(1),
                     [[
                         'Off', 'Calibrate', 'Caster', 'OpSpaceForce',
                         'OpSpaceTraj', 'CartLocal', 'CartGlob'
                     ], 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiModes', 'TrajMode', (self, 'traj_mode'), range(1),
                     [['Off', 'Joystick', 'Goal', 'Vias'], 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiModes', 'Caster Steer/Roll',
                     (self, 'steer_or_roll_mode'), range(1),
                     [['Steer', 'Roll'], 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiModes', 'CasterCtrlIdx', (self, 'caster_ctrl_idx'),
                     range(1), [['0', '1', '2', '3'], 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiModes', 'CasterCtrlMode',
                     (self, 'caster_ctrl_mode'), range(1),
                     [['Off', 'Tq', 'Vel'], 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiModes', 'JoyButton', (self, 'joy_button'), range(1),
                     [['Tri', 'Circ', 'Sqr', 'X'], 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders',
                     'JoystickX', (self, 'joy_x'),
                     range(1), [-1, 1],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'JoystickY', (self, 'joy_y'),
                     range(1), [-1, 1],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'JoystickYaw', (self, 'joy_yaw'),
                     range(1), [-1, 1],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'LocalVelX (m/s)', (self, 'local_velocity_desired_x'),
                     range(1), [-1, 1],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'LocalVelY (m/s)', (self, 'local_velocity_desired_y'),
                     range(1), [-1, 1],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'LocalVelHeading (deg/s)',
                     (self, 'local_velocity_desired_heading'),
                     range(1), [-60, 60],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'OpForceX (N)', (self, 'op_force_desired_x'),
                     range(1), [-1000, 1000],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'OpForceY (N)', (self, 'op_force_desired_y'),
                     range(1), [-1000, 1000],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'OpTorque (Nm)', (self, 'op_torque_desired'),
                     range(1), [-50, 50],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'Jnt Torque (Nm)', (self, 'joint_torque'),
                     range(1), [-12.8, 12.8],
                     m3g.M3GuiWrite,
                     column=1)
        #		self.gui.add('M3GuiSliders','Jnt Torque (Nm)', (self,'joint_torque'),range(1),[-6.4,6.4],m3g.M3GuiWrite,column=1)
        self.gui.add('M3GuiSliders',
                     'Jnt Vel (deg/s)', (self, 'joint_vel'),
                     range(1), [-90, 90],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'Goal X (m)', (self, 'goal_x'),
                     range(1), [-2, 2],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'Goal Y (m)', (self, 'goal_y'),
                     range(1), [-2, 2],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'Goal Yaw(deg)', (self, 'goal_yaw'),
                     range(1), [-180, 180],
                     m3g.M3GuiWrite,
                     column=1)
        time.sleep(0.5)
        self.omni.set_local_position(0, 0, 0, self.proxy)
        self.omni.set_global_position(0, 0, 0, self.proxy)
        self.gui.start(self.step)
bot = m3f.create_component(bot_name)

if rviz == True:
    viz = m3v.M3Viz(proxy, bot)

proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
bot.set_motor_power_on()
proxy.step()

if rviz == True:
    viz.step()

print 'Select chains:'
chains = m3t.user_select_components_interactive(bot.get_available_chains(),
                                                single=False)
# ######################################################
#Hardcode defaults for now...should move to config file
stiffness = {
    'right_arm': [
        0.5,  #J0
        0.5,  #J1
        0.5,  #J2
        0.5,  #J3
        0.5,  #J4
        0.5,  #J5
        0.5
    ],  #J6
    'left_arm': [
        0.5,  #J0
        0.5,  #J1
    def start(self):
        self.proxy.start()
        joint_names = self.proxy.get_joint_components()
        if len(joint_names) == 0:
            print 'No joint components available'
            self.proxy.stop()
            exit()
        joint_names = m3t.user_select_components_interactive(joint_names)
        actuator_ec_names = []
        actuator_names = []
        ctrl_names = []
        for n in joint_names:
            ctrl = m3t.get_joint_ctrl_component_name(n)
            if ctrl != "":
                ctrl_names.append(ctrl)
            actuator = m3t.get_joint_actuator_component_name(n)
            if actuator != "":
                actuator_names.append(actuator)
                actuator_ec = m3t.get_actuator_ec_component_name(actuator)
                if actuator_ec != "":
                    actuator_ec_names.append(actuator_ec)

        self.joint = []
        self.actuator_ec = []
        self.actuator = []
        self.ctrl = []

        for n in actuator_ec_names:
            c = m3f.create_component(n)
            if c is not None:
                try:
                    self.actuator_ec.append(c)
                    self.proxy.subscribe_status(self.actuator_ec[-1])
                    self.proxy.publish_param(self.actuator_ec[-1])
                except:
                    print 'Component', n, 'not available'

        for n in actuator_names:
            c = m3f.create_component(n)
            if c is not None:
                self.actuator.append(c)
                self.proxy.subscribe_status(self.actuator[-1])
                self.proxy.publish_param(self.actuator[-1])

        for n in ctrl_names:
            c = m3f.create_component(n)
            if c is not None:
                self.ctrl.append(c)
                self.proxy.subscribe_status(self.ctrl[-1])
                self.proxy.publish_param(self.ctrl[-1])

        for n in joint_names:
            c = m3f.create_component(n)
            if c is not None:
                self.joint.append(c)
                self.proxy.subscribe_status(self.joint[-1])
                self.proxy.publish_command(self.joint[-1])
                self.proxy.publish_param(self.joint[-1])

        #Enable motor power
        pwr_rt = m3t.get_actuator_ec_pwr_component_name(actuator_ec_names[0])
        self.pwr = m3f.create_component(pwr_rt)
        if self.pwr is not None:
            self.proxy.publish_command(self.pwr)
            self.pwr.set_motor_power_on()

        #Start them all up
        self.proxy.make_operational_all()

        #Force safe-op of robot, etc are present
        types = ['m3humanoid', 'm3hand', 'm3gripper']
        for t in types:
            cc = self.proxy.get_available_components(t)
            for ccc in cc:
                self.proxy.make_safe_operational(ccc)

        #Force safe-op of chain so that gravity terms are computed
        self.chain = None
        if len(joint_names) > 0:
            for j in joint_names:
                chain_name = m3t.get_joint_chain_name(j)
                if chain_name != "":
                    self.proxy.make_safe_operational(chain_name)
                    #self.chain=m3f.create_component(chain_name)
                    #self.proxy.publish_param(self.chain) #allow to set payload
                    #Force safe-op of chain so that gravity terms are computed
                    dynamatics_name = m3t.get_chain_dynamatics_component_name(
                        chain_name)
                    if dynamatics_name != "":
                        self.proxy.make_safe_operational(dynamatics_name)
                        self.dyn = m3f.create_component(dynamatics_name)
                        self.proxy.publish_param(
                            self.dyn)  #allow to set payload

        #Force safe-op of robot so that gravity terms are computed
        robot_name = m3t.get_robot_name()
        if robot_name != "":
            try:
                self.proxy.make_safe_operational(robot_name)
                self.robot = m3f.create_component(robot_name)
                self.proxy.subscribe_status(self.robot)
                self.proxy.publish_param(self.robot)  #allow to set payload
            except:
                print 'Component', robot_name, 'not available'

        tmax = max([x.param.max_tq for x in self.actuator])
        tmin = min([x.param.min_tq for x in self.actuator])

        qmax = max([x.param.max_q for x in self.joint])
        qmin = min([x.param.min_q for x in self.joint])

        ## Plots
        self.scope_torque = []
        self.scope_theta = []
        self.scope_thetadot = []
        self.scope_thetadotdot = []
        self.scope_torquedot = []

        for i, name in zip(xrange(len(joint_names)), joint_names):
            self.scope_torque.append(
                m3t.M3ScopeN(xwidth=100, yrange=None, title='Torque'))
            self.scope_theta.append(
                m3t.M3ScopeN(xwidth=100, yrange=None, title='Theta'))
            self.scope_thetadot.append(
                m3t.M3ScopeN(xwidth=100, yrange=None, title='ThetaDot'))
            self.scope_thetadotdot.append(
                m3t.M3ScopeN(xwidth=100, yrange=None, title='ThetaDotDot'))
            self.scope_torquedot.append(
                m3t.M3ScopeN(xwidth=100, yrange=None, title='TorqueDot'))

        #Create gui
        self.mode = [0] * len(self.joint)
        self.tq_desire_a = [0] * len(self.joint)
        self.tq_desire_b = [0] * len(self.joint)
        self.pwm_desire = [0] * len(self.joint)
        self.theta_desire_a = [0] * len(self.joint)
        self.theta_desire_b = [0] * len(self.joint)
        self.thetadot_desire = [0] * len(self.joint)
        self.stiffness = [0] * len(self.joint)
        self.slew = [0] * len(self.joint)
        self.step_period = [2000.0] * len(self.joint)
        self.cycle_theta = False
        self.cycle_last_theta = False
        self.cycle_thetadot = False
        self.cycle_last_thetadot = False
        self.cycle_torque = False
        self.cycle_last_torque = False
        self.save = False
        self.do_scope_torque = False
        self.do_scope_torquedot = False
        self.do_scope_theta = False
        self.do_scope_thetadot = False
        self.do_scope_thetadotdot = False
        self.brake = False
        self.save_last = False
        self.status_dict = self.proxy.get_status_dict()
        self.param_dict = self.proxy.get_param_dict()
        self.gui.add('M3GuiTree',
                     'Status', (self, 'status_dict'), [], [],
                     m3g.M3GuiRead,
                     column=2)
        self.gui.add('M3GuiTree',
                     'Param', (self, 'param_dict'), [], [],
                     m3g.M3GuiWrite,
                     column=3)
        self.gui.add('M3GuiModes', 'Mode', (self, 'mode'),
                     range(len(self.joint)), [[
                         'Off', 'Pwm', 'Torque', 'Theta', 'Torque_GC',
                         'Theta_GC', 'Theta_MJ', 'Theta_GC_MJ', 'Pose',
                         'Torque_GRAV_MODEL', 'ThetaDot_GC', 'ThetaDot'
                     ], 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders', 'TorqueA (mNm)', (self, 'tq_desire_a'),
                     range(len(self.joint)), [tmin, tmax], m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders', 'TorqueB (mNm)', (self, 'tq_desire_b'),
                     range(len(self.joint)), [tmin, tmax], m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders', 'Pwm', (self, 'pwm_desire'),
                     range(len(self.joint)), [-3200, 3200], m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders',
                     'Theta A(Deg)', (self, 'theta_desire_a'),
                     range(len(self.joint)), [qmin, qmax],
                     m3g.M3GuiWrite,
                     column=2)
        self.gui.add('M3GuiSliders',
                     'Theta B(Deg)', (self, 'theta_desire_b'),
                     range(len(self.joint)), [qmin, qmax],
                     m3g.M3GuiWrite,
                     column=2)
        self.gui.add('M3GuiSliders',
                     'Thetadot (Deg)', (self, 'thetadot_desire'),
                     range(len(self.joint)), [-120.0, 120.0],
                     m3g.M3GuiWrite,
                     column=2)
        self.gui.add('M3GuiSliders',
                     'Stiffness ', (self, 'stiffness'),
                     range(len(self.joint)), [0, 100],
                     m3g.M3GuiWrite,
                     column=3)
        self.gui.add('M3GuiSliders',
                     'Slew ', (self, 'slew'),
                     range(len(self.joint)), [0, 100],
                     m3g.M3GuiWrite,
                     column=3)
        self.gui.add('M3GuiToggle', 'Save', (self, 'save'), [],
                     [['On', 'Off']], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeTorque', (self, 'do_scope_torque'),
                     [], [['On', 'Off']], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeTorqueDot',
                     (self, 'do_scope_torquedot'), [], [['On', 'Off']],
                     m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeTheta', (self, 'do_scope_theta'), [],
                     [['On', 'Off']], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeThetaDot',
                     (self, 'do_scope_thetadot'), [], [['On', 'Off']],
                     m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ScopeThetaDotDot',
                     (self, 'do_scope_thetadotdot'), [], [['On', 'Off']],
                     m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders', 'StepPeriod (ms) ', (self, 'step_period'),
                     range(len(self.joint)), [0, 8000], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'CycleTheta', (self, 'cycle_theta'), [],
                     [['On', 'Off']], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'CycleThetaDot', (self, 'cycle_thetadot'),
                     [], [['On', 'Off']], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'CycleTorque', (self, 'cycle_torque'), [],
                     [['On', 'Off']], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'Brake', (self, 'brake'), [],
                     [['On', 'Off']], m3g.M3GuiWrite)
        self.gui.start(self.step)
	def start(self):
		self.proxy.start()
		cnames=self.proxy.get_available_components('m3actuator_ec')
		self.names=m3t.user_select_components_interactive(cnames)
		if len(self.names)==0:
			return
		self.actuator_ec=[]
		for name in self.names:
			self.actuator_ec.append(m3f.create_component(name))
			self.proxy.subscribe_status(self.actuator_ec[-1])
			self.proxy.publish_command(self.actuator_ec[-1]) 
			self.proxy.publish_param(self.actuator_ec[-1]) 
			self.proxy.make_operational(name)
		
		#pwr_ec=self.proxy.get_available_components('m3pwr_ec')
		#pwr_rt=self.proxy.get_available_components('m3pwr')
		#print 'A',pwr_rt[0],pwr_ec[0]
		#if len(pwr_rt):
			#pr=m3f.create_component(pwr_rt[0])
			#self.proxy.publish_command(pr)
			#self.proxy.make_operational(pwr_rt[0])
			#self.proxy.make_operational(pwr_ec[0])
			#pr.set_motor_power_on()
			
		pwr_rt=m3t.get_actuator_ec_pwr_component_name(self.names[0])
		pwr_ec=pwr_rt.replace('m3pwr','m3pwr_ec')
		pr=m3f.create_component(pwr_rt)
		self.proxy.publish_command(pr)
		self.proxy.make_operational(pwr_rt)
		self.proxy.make_operational(pwr_ec)
		pr.set_motor_power_on()
		
		tmax=[x.param.t_max for x in self.actuator_ec]
		tmin=[x.param.t_min for x in self.actuator_ec]
		
		
		self.proxy.step()
		for c in self.actuator_ec:
			self.bias.append(c.status.adc_torque)
		tl=min(tmin)-self.bias[0]
		tu=max(tmax)-self.bias[0]
		
		self.cycle_pwm=False
		self.cycle_last_pwm=False
		self.cycle_tq=False
		self.cycle_last_tq=False
		self.step_period=[2000.0]*len(self.actuator_ec)
		self.brake=[0]
		#Create gui
		self.mode=[0]*len(self.actuator_ec)
		self.t_desire_a=[0]*len(self.actuator_ec)
		self.t_desire_b=[0]*len(self.actuator_ec)
		self.pwm_desire_a=[0]*len(self.actuator_ec)
		self.pwm_desire_b=[0]*len(self.actuator_ec)
		self.current_desire_a=[0]*len(self.actuator_ec)
		self.current_desire_b=[0]*len(self.actuator_ec)
		self.save=False
		self.save_last=False
		self.do_scope_torque=False
		self.scope_torque=None
		self.status_dict=self.proxy.get_status_dict()
		self.param_dict=self.proxy.get_param_dict()
		self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
		self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
		self.gui.add('M3GuiModes',  'Mode',      (self,'mode'),range(len(self.actuator_ec)),[['Off','Pwm','PID','CURRENT'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'Brake',      (self,'brake'),range(1),[['Enabled','Disabled'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','tqDesire',  (self,'t_desire_a'),range(len(self.actuator_ec)),[tl,tu],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','tqDesire',  (self,'t_desire_b'),range(len(self.actuator_ec)),[tl,tu],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','pwmDesireA', (self,'pwm_desire_a'),range(len(self.actuator_ec)),[-3200,3200],m3g.M3GuiWrite) 
		self.gui.add('M3GuiSliders','pwmDesireB', (self,'pwm_desire_b'),range(len(self.actuator_ec)),[-3200,3200],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','currentDesireA', (self,'current_desire_a'),range(len(self.actuator_ec)),[-100,100],m3g.M3GuiWrite) 
		self.gui.add('M3GuiSliders','currentDesireB', (self,'current_desire_b'),range(len(self.actuator_ec)),[-3200,3200],m3g.M3GuiWrite) 
		self.gui.add('M3GuiSliders','StepPeriod (ms) ', (self,'step_period'),range(len(self.actuator_ec)),[0,4000],m3g.M3GuiWrite) 
		self.gui.add('M3GuiToggle', 'CyclePwm',      (self,'cycle_pwm'),[],[['On','Off']],m3g.M3GuiWrite)	
		self.gui.add('M3GuiToggle', 'CycleTq',      (self,'cycle_tq'),[],[['On','Off']],m3g.M3GuiWrite)	
		self.gui.add('M3GuiToggle', 'Save',      (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.add('M3GuiToggle', 'Scope',      (self,'do_scope_torque'),[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.start(self.step)
Exemple #25
0
if len(humanoid_shm_names) > 0:
    proxy.make_safe_operational(humanoid_shm_names[0])

if not rviz == True:
    bot.set_motor_power_on()

proxy.step()
if rviz == True:
    viz.step()

stiffness = 0.5
step_delta = .002  #meters

print 'Select arm:'
arm_names = ['right_arm', 'left_arm']
arm_name = m3t.user_select_components_interactive(arm_names, single=True)[0]

while True:
    proxy.step()
    print '--------------'
    print 's: set stiffness (Current', stiffness, ')'
    print 'd: set step delta (Current', step_delta, '(m))'
    print 'e: execute ijkt controller'
    print 'q: quit'
    print '--------------'
    print
    k = m3t.get_keystroke()
    if k == 'q':
        break
    if k == 's':
        print 'Enter stiffness (0-1.0) [', stiffness, ']'
Exemple #26
0
proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
    print "Error: no robot components found:", bot_name
    exit()
bot = m3f.create_component(bot_name)
proxy.publish_param(bot)  # allow to set payload
proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
bot.set_motor_power_on()
proxy.step()

chains = bot.get_available_chains()
print "Select chains to pose"
chains = m3t.user_select_components_interactive(chains)
menu = menu_thread(bot, chains)
menu.start()


slew = {}
qdes = {}
fn = m3t.get_m3_animation_path() + m3t.get_robot_name() + "_poser_config.yml"
f = file(fn, "r")
config = yaml.safe_load(f.read())
f.close()
stiffness = config["stiffness"]
delta_thresh = config["delta_thresh"]
slew_rate = config["slew_rate"]
proxy.step()
dbg = ""
	def start(self):
		# ######## Setup Proxy and Components #########################
		self.proxy.start()
		self.current_first = True
		

		
		bot_name=m3t.get_robot_name()
		if bot_name == "":
			print 'Error: no robot components found:', bot_names
			return
		self.bot=m3.humanoid.M3Humanoid(bot_name)	
		arm_names = self.bot.get_available_chains()	
		arm_names = [x for x in arm_names if x.find('arm')!=-1]
		if len(arm_names)==0:
			print 'No arms found'
			return
		if len(arm_names)==1:
			self.arm_name=arm_names[0]
		else:
			self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0]
			
		self.jnts = self.bot.get_joint_names(self.arm_name)
	    	
	    	comp={}
	    	
	    	for c in self.jnts:
		    comp[c]={'comp_rt':None,'comp_j':None,'torque_act':[],'torque_joint':[],'torque_gravity':[],'is_wrist':False}
		    if (c.find('j5')>=0 or c.find('j6')>=0):
			    comp[c]['is_wrist']=True
			    
	    	for c in self.jnts:		  
		    comp[c]['comp_j']=mcf.create_component(c)
		    comp[c]['comp_rt']=mcf.create_component(c.replace('joint','actuator'))
		    self.proxy.subscribe_status(comp[c]['comp_rt'])
		    self.proxy.subscribe_status(comp[c]['comp_j'])

		# ####### Setup Proxy #############
		self.proxy.subscribe_status(self.bot)
		self.proxy.publish_command(self.bot)
		self.proxy.make_operational_all()
		self.bot.set_motor_power_on()
		self.ndof=self.bot.get_num_dof(self.arm_name)
		
		humanoid_shm_names=self.proxy.get_available_components('m3humanoid_shm')
		if len(humanoid_shm_names) > 0:
		  self.proxy.make_safe_operational(humanoid_shm_names[0])
		  
		self.bot.set_mode_off(self.arm_name)

		print 'This script will calibrate the zero-torque of the A2 arm while posed using current torque_gravity estimate'
		print '----------------------------------------------------------------------------------------------------------'
		print 'With E-Stop down, pose the arm in desired position to calibrate torque zeroes around.'
		print 'Press any key when ready posed.'
		
		raw_input()   
		
		time.sleep(0.5)
		
		self.proxy.step()

		self.theta_curr = self.bot.get_theta_deg(self.arm_name)[:]			
		
		self.proxy.step()

		print 'Posed position set.'
		print 'Release E-stop and press any key for arm to hold pose.'
		
		raw_input()   
		
		self.bot.set_mode_theta_gc(self.arm_name)
		self.bot.set_theta_deg(self.arm_name,self.theta_curr)
		self.bot.set_stiffness(self.arm_name,[1.0]*7)
		self.bot.set_slew_rate_proportion(self.arm_name,[1.0]*self.ndof)
		
		self.proxy.step()

		print 'Press any key to start torque calibration for all joints.'		
		
		raw_input()   
		
		self.proxy.step()

		# ###########################
		ns=30
		for i in range(ns):
			self.proxy.step()
			print '---------'
			for c in comp.keys():
				tqj=comp[c]['comp_j'].get_torque_mNm()
				tqg=comp[c]['comp_j'].get_torque_gravity_mNm()/1000.0
				tqa=comp[c]['comp_rt'].get_torque_mNm()
				comp[c]['torque_act'].append(tqa)
				comp[c]['torque_joint'].append(tqj)
				comp[c]['torque_gravity'].append(tqg)
				if comp[c]['is_wrist']:
					print c,':joint',tqj,':gravity',tqg,':actuator',tqa
				else:
					print c,':joint',tqj,':gravity',tqg,
			time.sleep(0.05)

		do_query = True
			
		# ###########################
		for c in comp.keys():
			print '--------',c,'---------'
			tqg=float(na.array(comp[c]['torque_gravity'],na.Float32).mean())
			tqj=float(na.array(comp[c]['torque_joint'],na.Float32).mean())
			tqa=float(na.array(comp[c]['torque_act'],na.Float32).mean())
			if not comp[c]['is_wrist']:
				bias=tqa+tqg
				torque=M3TorqueSensor(comp[c]['comp_rt'].config['calib']['torque']['type'])
				print 'Measured torque:',tqa,'Torque gravity:', tqg
				print 'Delta of',bias,'mNm'
				comp[c]['comp_rt'].config['calib']['torque']['cb_bias']=comp[c]['comp_rt'].config['calib']['torque']['cb_bias']-bias
				comp[c]['comp_rt'].config['calibration_date']=time.asctime()
				if do_query:
					print 'Save calibration? [y]'
					if m3t.get_yes_no('y'):
						comp[c]['comp_rt'].write_config()
				else:
					comp[c]['comp_rt'].write_config()
			else: 
				print 'Wrist joint...'
				if c.find('j5')!=-1: #do j5/j6 at once
					cc=None
					for x in comp.keys():
						if x.find('j6')!=-1:
							cc=x
					if cc is None:
						print 'Did not find coupled joint to',c
					tqg_c=float(na.array(comp[cc]['torque_gravity'],na.Float32).mean())
					tqj_c=float(na.array(comp[cc]['torque_joint'],na.Float32).mean())
					tqa_c=float(na.array(comp[cc]['torque_act'],na.Float32).mean())
					x=comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][0] #Joint to actuator matrix
					y=comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][1]
					m=comp[cc]['comp_j'].config['transmission']['tqj_to_tqa'][0]
					n=comp[cc]['comp_j'].config['transmission']['tqj_to_tqa'][1]
					tqg_a5= x*tqg+y*tqg_c
					tqg_a6= m*tqg_c+n*tqg
					bias_5=tqa+tqg_a5
					bias_6=tqa_c+tqg_a6
					torque_5=M3TorqueSensor(comp[c]['comp_rt'].config['calib']['torque']['type'])
					torque_6=M3TorqueSensor(comp[cc]['comp_rt'].config['calib']['torque']['type'])
					print '------------'
					print 'J5: Previous joint torque',tqj,'with joint torque gravity', tqg
					print 'J5: Previous actuator torque',tqa,'with actuator torque gravity', tqg_a5
					print 'J5: Actuator delta of',bias_5,'mNm'
					print '------------'
					print 'J6: Previous joint torque',tqj_c,'with joint torque gravity', tqg_c
					print 'J6: Previous actuator torque',tqa_c,'with actuator torque gravity', tqg_a6
					print 'J6: Actuator delta of',bias_6,'mNm'
					print '------------'
					comp[c]['comp_rt'].config['calib']['torque']['cb_bias']=comp[c]['comp_rt'].config['calib']['torque']['cb_bias']-bias_5
					comp[c]['comp_rt'].config['calibration_date']=time.asctime()
					comp[cc]['comp_rt'].config['calib']['torque']['cb_bias']=comp[cc]['comp_rt'].config['calib']['torque']['cb_bias']-bias_6
					comp[cc]['comp_rt'].config['calibration_date']=time.asctime()
					if do_query:
						print 'Save calibration? [y]'
						if m3t.get_yes_no('y'):
							comp[c]['comp_rt'].write_config()
							comp[cc]['comp_rt'].write_config()
					else:
						comp[c]['comp_rt'].write_config()
						comp[cc]['comp_rt'].write_config()		
		
		self.bot.set_mode_off(self.arm_name)
		self.proxy.stop() 
	def start(self):
		self.proxy.start()
		#print 'Enable RVIZ [n]?'
		self.rviz = False
		#if m3t.get_yes_no('n'):
		#	self.rviz = True
			
		chain_names=self.proxy.get_chain_components()
		self.chain=[]
		if len(chain_names)>0:
			print 'Select kinematic chain'
			self.chain_names=m3t.user_select_components_interactive(chain_names,single=True)
		self.chain.append(m3f.create_component(self.chain_names[0]))
		self.proxy.subscribe_status(self.chain[-1])
		self.limb=m3t.get_chain_limb_name(self.chain_names[0])
		
		joint_names=m3t.get_chain_joint_names(self.chain_names[0])
		print 'Select joint'
		joint_names=m3t.user_select_components_interactive(joint_names,single=True)
		self.joint=[]
		self.actuator=[]
		self.actuator_ec=[]
		acutator_names=[]
		for n in joint_names:
			self.joint.append(m3f.create_component(n))			
			actuator_name = m3t.get_joint_actuator_component_name(n)
			actuator_ec_name = m3t.get_actuator_ec_component_name(actuator_name)
			self.actuator.append(m3f.create_component(actuator_name))
			self.actuator_ec.append(m3f.create_component(actuator_ec_name))
			self.proxy.subscribe_status(self.joint[-1])
			self.proxy.publish_param(self.joint[-1])
			self.proxy.subscribe_status(self.actuator[-1])
			self.proxy.publish_param(self.actuator[-1])
			if self.actuator_ec[0] is not None:
				self.proxy.subscribe_status(self.actuator_ec[-1])
				self.proxy.publish_param(self.actuator_ec[-1])
		
		
		#kine_names=self.proxy.get_available_components('m3dynamatics')
		self.kine = []
		#if len(kine_names)>0:
			#print 'Select dynamatics controller'
			#kine_names=m3t.user_select_components_interactive(kine_names)
			
		#for n in kine_names:
			#self.kine.append(m3f.create_component(n))			
			#self.proxy.subscribe_status(self.kine[-1])
	
		bot_name=m3t.get_robot_name()
		if bot_name == "":
			print 'Error: no robot components found:', bot_names
			return
		self.bot=m3f.create_component(bot_name)
				
		if self.rviz:			
			self.viz = m3v.M3Viz(self.proxy, self.bot)			
		
		#self.proxy.publish_param(self.bot) #allow to set payload
		#self.proxy.subscribe_status(self.bot)
		#self.proxy.publish_command(self.bot)		
		#self.proxy.make_operational_all()
		self.bot.initialize(self.proxy)
		#self.chain_names = self.bot.get_available_chains()
		#Create gui
		self.mode=[0]
		self.posture=[0]
		self.theta_desire_a=[0]*self.bot.get_num_dof(self.limb)
		self.theta_desire_b=[0]*self.bot.get_num_dof(self.limb)
		self.stiffness=[50.0]*self.bot.get_num_dof(self.limb)
		self.thetadot=[10.0]*self.bot.get_num_dof(self.limb)
		
		#print 'Selected: ',self.chain_names[0],self.limb,self.bot.get_num_dof(self.limb)
		
		#self.slew=[0]
		self.save=False
		self.save_last=False
		self.status_dict=self.proxy.get_status_dict()
		self.param_dict=self.proxy.get_param_dict()
		self.gui.add('M3GuiModes',  'Mode',      (self,'mode'),range(1),[['Off','Pwm','Torque','Theta','Torque_GC','Theta_GC','Theta_MJ', 'Theta_GC_MJ'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=1)
		self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=1)
		
		self.gui.add('M3GuiSliders','ThetaA (Deg)', (self,'theta_desire_a'),range(len(self.theta_desire_a)),[-45,140],m3g.M3GuiWrite,column=2)
		self.gui.add('M3GuiModes',  'Posture',      (self,'posture'),range(1),[['A','B','Cycle'],1],m3g.M3GuiWrite,column=2)
		self.gui.add('M3GuiSliders','ThetaB (Deg)', (self,'theta_desire_b'),range(len(self.theta_desire_b)),[-45,140],m3g.M3GuiWrite,column=2)
		
		self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.stiffness)),[0,100],m3g.M3GuiWrite,column=3)
		self.gui.add('M3GuiToggle', 'Save',      (self,'save'),[],[['On','Off']],m3g.M3GuiWrite,column=3)
		self.gui.add('M3GuiSliders','ThetaDot ', (self,'thetadot'),range(len(self.thetadot)),[0,100],m3g.M3GuiWrite,column=3)
		
		#self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.sea_joint)),[0,100],m3g.M3GuiWrite,column=3) 
		

		self.gui.start(self.step)
Exemple #29
0
#INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES INCLUDING,
#BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
#CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#POSSIBILITY OF SUCH DAMAGE.

import m3.toolbox as m3t
import m3.toolbox_ctrl as m3tc
import time

proxy = m3p.M3RtProxy()

proxy.start()
name_ec = m3t.user_select_components_interactive(
    proxy.get_available_components('m3actuator_ec'), single=True)
if name_ec is None:
    exit()
comp = m3f.create_component(name_ec)
proxy.subscribe_status(comp)
proxy.make_operational(name_ec)
q_log = []
tq_log = []
print 'Ready to generate motion? Hit any key to start'
m3t.get_keystroke()
ts = time.time()
try:
    while time.time() - ts > 5.0:
        proxy.step()
        q = comp.status.qei_on
        tq = comp.status.adc_torque
#You should have received a copy of the GNU Lesser General Public License
#along with M3.  If not, see <http://www.gnu.org/licenses/>.

import m3.rt_proxy as m3p
import m3.pwr as m3rt
import m3.pwr_ec as m3e
import m3.toolbox as m3t
import m3.omnibase as m3o
import time

proxy = m3p.M3RtProxy()
proxy.start()
pwr_name=proxy.get_available_components('m3pwr')
if len(pwr_name)>1:
            pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)

pwr=m3rt.M3Pwr(pwr_name[0])

proxy.subscribe_status(pwr)

base_name=proxy.get_available_components('m3omnibase')
if len(base_name)!=1:
            print 'Invalid number of base components available'
            proxy.stop()
            exit()
omni=m3o.M3OmniBase(base_name[0])
proxy.subscribe_status(omni)

scope_steer = m3t.M3ScopeN(xwidth=100,title='Steer and Total Currents')
scope_roll = m3t.M3ScopeN(xwidth=100,title='Roll Currents')
bot = m3f.create_component(bot_name)

if rviz == True:
    viz = m3v.M3Viz(proxy, bot)

proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
bot.set_motor_power_on()
proxy.step()

if rviz == True:
    viz.step()

print "Select chains:"
chains = m3t.user_select_components_interactive(bot.get_available_chains(), single=False)
# ######################################################
# Hardcode defaults for now...should move to config file
stiffness = {
    "right_arm": [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5],  # J0  # J1  # J2  # J3  # J4  # J5  # J6
    "left_arm": [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5],  # J0  # J1  # J2  # J3  # J4  # J5  # J6
    "torso": [0.65, 0.65, 0.65],  # J0  # J1
}  # J2
# Deg/S
vel_avg = {
    "right_arm": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0],  # J0  # J1  # J2  # J3  # J4  # J5  # J6
    "left_arm": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0],  # J0  # J1  # J2  # J3  # J4  # J5  # J6
    "torso": [5.0, 5.0, 5.0],  # J0  # J1
}  # J2
scale_stiffness = {"right_arm": 1.0, "left_arm": 1.0, "torso": 1.0}
scale_vel_avg = {"right_arm": 1.0, "left_arm": 1.0, "torso": 1.0}
def get_boards():
    print 'Finding burnable board from m3_config.yml...'
    name_ec = m3t.get_ec_component_names()
    boards = {}
    for n in name_ec:
        config = m3t.get_component_config(n)
        try:
            fw = config['firmware'] + '.hex'
        except KeyError:
            print 'Missing firmware key for', n, '...skipping'
            fw = None
        sn = config['ethercat']['serial_number']
        try:
            chid = config['chid']
        except KeyError:
            chid = 0
        ssn = get_slave_sn()
        sid = None
        for s in ssn:
            if sn == s[0]:
                sid = s[1]
        ff = m3t.get_m3_config_path() + 'eeprom/eeprom*_sn_' + str(sn) + '.hex'
        eepf = glob.glob(ff)
        if len(eepf) == 0:
            print 'EEPROM file not found', ff, 'skipping board...'
        if len(eepf) > 1:
            print 'Multiple eeproms found for', ff
            print 'Select correct eeprom ID'
            for i in range(len(eepf)):
                print i, ' : ', eepf[i]
            idx = m3t.get_int()
            eepf = [eepf[idx]]
        if sid != None and len(eepf) == 1 and fw is not None and chid == 0:
            boards[n] = {
                'firmware': fw,
                'sn': sn,
                'slave_id': sid,
                'eeprom': eepf[0],
                'connected': False,
                'write_success': False
            }

    print 'Found the following valid configurations: '
    print boards.keys()
    print
    print 'Selection type'
    print '-----------------'
    print 'a: all boards'
    print 's: single board'
    print 'm: multiple boards'
    t = raw_input()
    if t != 'a' and t != 's' and t != 'm':
        print 'Invalid selection'
        return {}
    rem = []
    if t == 'm':
        for k in boards.keys():
            print '-----------------------'
            print 'Burn ', k, 'board [y]?'
            if not m3t.get_yes_no('y'):
                rem.append(k)
        for r in rem:
            boards.pop(r)
    if t == 's':
        k = m3t.user_select_components_interactive(boards.keys(), single=True)
        if len(k) == 0:
            return {}
        boards = {k[0]: boards[k[0]]}

    print '-------------------------------'
    print 'Burning the following boards: '
    for k in boards.keys():
        print k
    return boards
Exemple #33
0
    def start(self):
        self.proxy.start()
        self.proxy.make_operational_all()

        chain_names = self.proxy.get_available_components('m3hand')
        if len(chain_names) > 1:
            hand_name = m3t.user_select_components_interactive(chain_names,
                                                               single=True)
        else:
            hand_name = chain_names
        pwr_name = self.proxy.get_available_components('m3pwr')
        if len(pwr_name) > 1:
            pwr_name = m3t.user_select_components_interactive(pwr_name,
                                                              single=True)

        print 'Position arm [y]?'
        if m3t.get_yes_no('y'):
            arm_names = self.proxy.get_available_components('m3arm')
            if len(arm_names) > 1:
                print 'Select arm: '
                arm_name = m3t.user_select_components_interactive(
                    arm_names, single=True)[0]
            else:
                arm_name = arm_names[0]
            self.arm = m3f.create_component(arm_name)
            self.proxy.publish_command(self.arm)
            self.arm.set_mode_theta_gc()
            self.arm.set_theta_deg([30, 0, 0, 110, 0, 0, 0])
            self.arm.set_stiffness(0.5)
            self.arm.set_slew_rate_proportion([0.75] * 7)

        self.chain = m3f.create_component(hand_name[0])
        self.proxy.publish_command(self.chain)
        self.proxy.subscribe_status(self.chain)

        self.pwr = m3f.create_component(pwr_name[0])
        self.proxy.publish_command(self.pwr)
        self.pwr.set_motor_power_on()

        #Force safe-op of robot if present
        hum = self.proxy.get_available_components('m3humanoid')
        if len(hum) > 0:
            self.proxy.make_safe_operational(hum[0])

        #Setup postures
        self.posture_filename = m3t.get_m3_animation_path(
        ) + self.chain.name + '_postures.yml'
        f = file(self.posture_filename, 'r')
        self.data = yaml.safe_load(f.read())
        self.param = self.data['param']
        f.close()
        self.theta_desire = [0, 0, 0, 0, 0]
        self.mode = [1, 1, 1, 1, 1]

        #Create gui
        self.run = False
        self.run_last = False
        self.running = False
        self.grasp = False
        self.grasp_last = False
        self.grasp_off = False
        self.grasp_off_ts = time.time()
        self.status_dict = self.proxy.get_status_dict()
        self.gui.add('M3GuiTree',
                     'Status', (self, 'status_dict'), [], [],
                     m3g.M3GuiRead,
                     column=3)
        self.gui.add('M3GuiTree',
                     'Param', (self, 'param'), [], [],
                     m3g.M3GuiWrite,
                     column=3)
        self.gui.add('M3GuiToggle',
                     'Animation', (self, 'run'), [], [['Run', 'Stop']],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiModes',
                     'Joint', (self, 'mode'),
                     range(5), [['Off', 'Enabled'], 1],
                     m3g.M3GuiWrite,
                     column=2)
        self.gui.add('M3GuiSliders',
                     'Theta (Deg)', (self, 'theta_desire'),
                     range(5), [0, 300],
                     m3g.M3GuiWrite,
                     column=2)
        self.gui.add('M3GuiToggle',
                     'Power Grasp', (self, 'grasp'), [], [['Run', 'Stop']],
                     m3g.M3GuiWrite,
                     column=2)
        self.gui.start(self.step)
Exemple #34
0
    def start(self):
        # ######## Setup Proxy and Components #########################
        self.proxy.start()
        self.current_first = True

        bot_name = m3t.get_robot_name()
        if bot_name == "":
            print 'Error: no robot components found:', bot_names
            return
        self.bot = m3.humanoid.M3Humanoid(bot_name)
        arm_names = self.bot.get_available_chains()
        arm_names = [x for x in arm_names if x.find('arm') != -1]
        if len(arm_names) == 0:
            print 'No arms found'
            return
        if len(arm_names) == 1:
            self.arm_name = arm_names[0]
        else:
            self.arm_name = m3t.user_select_components_interactive(
                arm_names, single=True)[0]

        # ####### Setup Hand #############
        hand_names = self.proxy.get_available_components('m3hand')
        hand_name = ''
        if len(hand_names) > 1:
            hand_name = m3t.user_select_components_interactive(chain_names,
                                                               single=True)
        if len(hand_names) == 1:
            hand_name = hand_names[0]
        if len(hand_name):
            self.hand = m3.hand.M3Hand(hand_name)
            self.proxy.publish_command(self.hand)
            self.proxy.subscribe_status(self.hand)
        else:
            self.hand = None
        # ####### Setup Proxy #############
        self.proxy.subscribe_status(self.bot)
        self.proxy.publish_command(self.bot)
        self.proxy.make_operational_all()
        self.bot.set_motor_power_on()
        self.ndof = self.bot.get_num_dof(self.arm_name)
        self.via_traj = {}
        self.via_traj_first = True
        self.theta_curr = [0.0] * self.ndof
        # ######## Square/Circle stuff #########################
        if self.arm_name == 'right_arm':
            self.center = [0.450, -0.28, -0.1745]
        else:
            self.center = [0.450, 0.28, -0.1745]
        avail_chains = self.bot.get_available_chains()
        for c in avail_chains:
            if c == 'torso':
                self.center[2] += 0.5079

        self.jt = m3jt.JointTrajectory(7)
        self.axis_demo = [0, 0, 1]

        # ##### Generate square vias ############################
        ik_vias = []
        length_m = 25 / 100.0
        resolution = 20
        y_left = self.center[1] + length_m / 2.0
        y_right = self.center[1] - length_m / 2.0
        z_top = self.center[2] + length_m / 2.0
        z_bottom = self.center[2] - length_m / 2.0
        dy = (y_left - y_right) / nu.float(resolution)
        dz = (z_top - z_bottom) / nu.float(resolution)
        x = self.center[0]
        if self.arm_name == 'right_arm':
            # first add start point
            ik_vias.append([
                x, y_left, z_top, self.axis_demo[0], self.axis_demo[1],
                self.axis_demo[2]
            ])
            # add top line
            for i in range(resolution):
                ik_vias.append([
                    x, y_left - (i + 1) * dy, z_top, self.axis_demo[0],
                    self.axis_demo[1], self.axis_demo[2]
                ])
            # add right line
            for i in range(resolution):
                ik_vias.append([
                    x, y_right, z_top - (i + 1) * dz, self.axis_demo[0],
                    self.axis_demo[1], self.axis_demo[2]
                ])
            # add bottom line
            for i in range(resolution):
                ik_vias.append([
                    x, y_right + (i + 1) * dy, z_bottom, self.axis_demo[0],
                    self.axis_demo[1], self.axis_demo[2]
                ])
            # add left line
            for i in range(resolution):
                ik_vias.append([
                    x, y_left, z_bottom + (i + 1) * dz, self.axis_demo[0],
                    self.axis_demo[1], self.axis_demo[2]
                ])
        else:
            # first add start point
            ik_vias.append([
                x, y_right, z_top, self.axis_demo[0], self.axis_demo[1],
                self.axis_demo[2]
            ])
            # add top line
            for i in range(resolution):
                ik_vias.append([
                    x, y_right + (i + 1) * dy, z_top, self.axis_demo[0],
                    self.axis_demo[1], self.axis_demo[2]
                ])
            # add right line
            for i in range(resolution):
                ik_vias.append([
                    x, y_left, z_top - (i + 1) * dz, self.axis_demo[0],
                    self.axis_demo[1], self.axis_demo[2]
                ])
            # add bottom line
            for i in range(resolution):
                ik_vias.append([
                    x, y_left - (i + 1) * dy, z_bottom, self.axis_demo[0],
                    self.axis_demo[1], self.axis_demo[2]
                ])
            # add left line
            for i in range(resolution):
                ik_vias.append([
                    x, y_right, z_bottom + (i + 1) * dz, self.axis_demo[0],
                    self.axis_demo[1], self.axis_demo[2]
                ])
        self.via_traj['Square'] = []
        # use zero position as reference for IK solver
        self.bot.set_theta_sim_deg(self.arm_name,
                                   [0] * self.bot.get_num_dof(self.arm_name))
        for ikv in ik_vias:
            theta_soln = []
            #print 'solving for square ik via:', ikv
            if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3],
                                                      ikv[3:], theta_soln):
                self.via_traj['Square'].append(theta_soln)
                self.bot.set_theta_sim_deg(self.arm_name, theta_soln)
            else:
                print 'WARNING: no IK solution found for via ', ikv
        self.bot.set_theta_sim_deg(self.arm_name,
                                   [0] * self.bot.get_num_dof(self.arm_name))

        # ##### Generate circle vias ############################
        ik_vias = []
        diameter_m = 25.0 / 100.0
        resolution = 20
        x = self.center[0]
        for i in range(resolution * 4 + 1):
            dt = 2 * nu.pi / (nu.float(resolution) * 4)
            t = (nu.pi / 2) + i * dt
            if t > nu.pi:
                t -= 2 * nu.pi
            y = self.center[1] + (diameter_m / 2.0) * nu.cos(t)
            z = self.center[2] + (diameter_m / 2.0) * nu.sin(t)
            ik_vias.append([
                x, y, z, self.axis_demo[0], self.axis_demo[1],
                self.axis_demo[2]
            ])
        self.via_traj['Circle'] = []
        # use zero position as reference for IK solver
        self.bot.set_theta_sim_deg(self.arm_name,
                                   [0] * self.bot.get_num_dof(self.arm_name))
        for ikv in ik_vias:
            theta_soln = []
            if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3],
                                                      ikv[3:], theta_soln):
                self.via_traj['Circle'].append(theta_soln)
                self.bot.set_theta_sim_deg(self.arm_name, theta_soln)
            else:
                print 'WARNING: no IK solution found for via ', ikv
        self.bot.set_theta_sim_deg(self.arm_name,
                                   [0] * self.bot.get_num_dof(self.arm_name))

        # ##### Load Via Trajectories ############################
        self.via_files = {'TrajA': 'kha1.via'}
        pt = m3t.get_m3_animation_path()
        for k in self.via_files.keys():
            fn = pt + self.via_files[k]
            try:
                f = file(fn, 'r')
                d = yaml.safe_load(f.read())
                self.via_traj[k] = d[self.arm_name]
            except IOError:
                print 'Via file', k, 'not present. Skipping...'

        # ##### Hand Trajectories ############################
        if self.hand is not None:
            pf = m3t.get_m3_animation_path() + hand_name + '_postures.yml'
            f = file(pf, 'r')
            self.hand_data = yaml.safe_load(f.read())
            f.close()
        self.hand_traj_first = True
        # ######## Demo and GUI #########################
        self.off = False
        self.grasp = False
        self.arm_mode_names = [
            'Off', 'Zero', 'Current', 'HoldUp', 'Square', 'Circle', 'TrajA'
        ]
        self.hand_mode_names = ['Off', 'Open', 'Grasp', 'Animation']
        self.arm_mode_methods = [
            self.step_off, self.step_zero, self.step_current,
            self.step_hold_up, self.step_via_traj, self.step_via_traj,
            self.step_via_traj
        ]
        self.hand_mode_methods = [
            self.step_hand_off, self.step_hand_open, self.step_hand_grasp,
            self.step_hand_animation
        ]
        self.arm_mode = [0]
        self.hand_mode = [0]
        self.poses = {
            'zero': {
                'right_arm': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                'left_arm': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
            },
            'holdup': {
                'right_arm': [56.0, 26.0, -8.0, 84.0, 119.0, -36.0, 2.0],
                'left_arm': [56.0, -26.0, 8.0, 84.0, -119.0, -36.0, -2.0]
            }
        }
        self.stiffness = [50]
        self.velocity = [25]
        self.gui.add('M3GuiModes',
                     'Arm Mode', (self, 'arm_mode'),
                     range(1), [self.arm_mode_names, 1],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiToggle',
                     'ESTOP', (self, 'off'), [],
                     [['Arm Enabled', 'Arm Disabled']],
                     m3g.M3GuiWrite,
                     column=1)
        if self.hand is not None:
            self.gui.add('M3GuiToggle',
                         'Grasp', (self, 'grasp'), [],
                         [['GraspOpen', 'GraspClosed']],
                         m3g.M3GuiWrite,
                         column=1)
            self.gui.add('M3GuiModes',
                         'Hand Mode', (self, 'hand_mode'),
                         range(1), [self.hand_mode_names, 1],
                         m3g.M3GuiWrite,
                         column=1)
        self.gui.add('M3GuiSliders',
                     'Stiffness ', (self, 'stiffness'), [0], [0, 100],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.add('M3GuiSliders',
                     'Velocity ', (self, 'velocity'), [0], [0, 40],
                     m3g.M3GuiWrite,
                     column=1)
        self.gui.start(self.step)
    def start(self):
        self.proxy.start()
        self.proxy.make_operational_all()

        chain_names=self.proxy.get_available_components('m3hand')
        if len(chain_names)>1:
            hand_name=m3t.user_select_components_interactive(chain_names,single=True)
        else:
            hand_name=chain_names
        pwr_name=self.proxy.get_available_components('m3pwr')
	if len(pwr_name)>1:
            pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)

	print 'Position arm [y]?'
	if m3t.get_yes_no('y'):
		arm_names=self.proxy.get_available_components('m3arm')
		if len(arm_names)>1:
			print 'Select arm: '	
			arm_name=m3t.user_select_components_interactive(arm_names,single=True)[0]
		else:
			arm_name=arm_names[0]
		self.arm=m3f.create_component(arm_name)
		self.proxy.publish_command(self.arm)
		self.arm.set_mode_theta_gc()
		self.arm.set_theta_deg([30,0,0,110,0,0,0])
		self.arm.set_stiffness(0.5)
		self.arm.set_slew_rate_proportion([0.75]*7)
		
        self.chain=m3f.create_component(hand_name[0])
        self.proxy.publish_command(self.chain)
        self.proxy.subscribe_status(self.chain)

        self.pwr=m3f.create_component(pwr_name[0])
        self.proxy.publish_command(self.pwr)
        self.pwr.set_motor_power_on()

 	#Force safe-op of robot if present
        hum=self.proxy.get_available_components('m3humanoid')
        if len(hum)>0:
            self.proxy.make_safe_operational(hum[0])
	
        #Setup postures
	self.posture_filename=m3t.get_m3_animation_path()+self.chain.name+'_postures.yml'
	f=file(self.posture_filename,'r')
	self.data= yaml.safe_load(f.read())
	self.param=self.data['param']
	f.close()
	self.theta_desire=[0,0,0,0,0]
	self.mode=[1,1,1,1,1]
	
        #Create gui
        self.run=False
        self.run_last=False
        self.running=False
	self.grasp=False
	self.grasp_last=False
	self.grasp_off=False
	self.grasp_off_ts=time.time()
        self.status_dict=self.proxy.get_status_dict()
        self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=3)
        self.gui.add('M3GuiTree',   'Param',   (self,'param'),[],[],m3g.M3GuiWrite,column=3)
        self.gui.add('M3GuiToggle', 'Animation',      (self,'run'),[],[['Run','Stop']],m3g.M3GuiWrite,column=1)
	self.gui.add('M3GuiModes',  'Joint',      (self,'mode'),range(5),[['Off','Enabled'],1],m3g.M3GuiWrite,column=2)	
	self.gui.add('M3GuiSliders','Theta (Deg)', (self,'theta_desire'),range(5),[0,300],m3g.M3GuiWrite,column=2) 
	self.gui.add('M3GuiToggle', 'Power Grasp', (self,'grasp'),[],[['Run','Stop']],m3g.M3GuiWrite,column=2)
        self.gui.start(self.step)
Exemple #36
0
 def start(self):
     self.ts = time.time()
     self.proxy.start()
     chain_names = self.proxy.get_available_components('m3ledx2_ec')
     led_name = m3t.user_select_components_interactive(chain_names,
                                                       single=True)[0]
     pwr_name = self.proxy.get_available_components('m3pwr')[0]
     pwr_ec_name = self.proxy.get_available_components('m3pwr_ec')[0]
     self.pwr = m3f.create_component(pwr_name)
     self.proxy.publish_command(self.pwr)
     self.led = m3f.create_component(led_name)
     self.proxy.publish_command(self.led)
     self.proxy.subscribe_status(self.led)
     self.proxy.make_operational(led_name)
     self.proxy.make_operational(pwr_name)
     self.proxy.make_operational(pwr_ec_name)
     self.proxy.subscribe_status(self.pwr)
     self.pwr.set_motor_power_on()
     self.branch_a_board_a_r = [0]
     self.branch_a_board_a_g = [0]
     self.branch_a_board_a_b = [0]
     self.branch_a_board_b_r = [0]
     self.branch_a_board_b_g = [0]
     self.branch_a_board_b_b = [0]
     self.branch_b_board_a_r = [0]
     self.branch_b_board_a_g = [0]
     self.branch_b_board_a_b = [0]
     self.branch_b_board_b_r = [0]
     self.branch_b_board_b_g = [0]
     self.branch_b_board_b_b = [0]
     self.slew = [10000]
     #Create gui
     self.run = False
     self.run_last = False
     self.enable = False
     self.pulse = False
     self.status_dict = self.proxy.get_status_dict()
     self.gui.add('M3GuiTree',
                  'Status', (self, 'status_dict'), [], [],
                  m3g.M3GuiRead,
                  column=3)
     self.gui.add('M3GuiToggle',
                  'Enable', (self, 'enable'), [], [['On', 'Off']],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiToggle',
                  'Pulse', (self, 'pulse'), [], [['On', 'Off']],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'Slew', (self, 'slew'),
                  range(1), [0, 25500],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchA.BoardA.R', (self, 'branch_a_board_a_r'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchA.BoardA.G', (self, 'branch_a_board_a_g'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchA.BoardA.B', (self, 'branch_a_board_a_b'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchA.BoardB.R', (self, 'branch_a_board_b_r'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchA.BoardB.G', (self, 'branch_a_board_b_g'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchA.BoardB.B', (self, 'branch_a_board_b_b'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchB.BoardA.R', (self, 'branch_b_board_a_r'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchB.BoardA.G', (self, 'branch_b_board_a_g'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchB.BoardA.B', (self, 'branch_b_board_a_b'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchB.BoardB.R', (self, 'branch_b_board_b_r'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchB.BoardB.G', (self, 'branch_b_board_b_g'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.add('M3GuiSliders',
                  'BranchB.BoardB.B', (self, 'branch_b_board_b_b'),
                  range(1), [0, 1023],
                  m3g.M3GuiWrite,
                  column=1)
     self.gui.start(self.step)
if bot_name == "":
	print 'Error: no robot components found:', bot_name
bot=m3f.create_component(bot_name)
proxy.publish_param(bot) #allow to set payload
proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
bot.set_motor_power_on()

humanoid_shm_names=proxy.get_available_components('m3humanoid_shm')
if len(humanoid_shm_names) > 0:
  proxy.make_safe_operational(humanoid_shm_names[0])

chains=bot.get_available_chains()
print 'Select chain'
chains=m3t.user_select_components_interactive(chains,single=True)

stiffness=0.5
print 'Enter stiffness (0-1.0) [',stiffness,']'
stiffness=max(0,min(1.0,m3t.get_float(stiffness)))

for c in chains:
	ndof=bot.get_num_dof(c)
	bot.set_mode_pose(c)	
	bot.set_stiffness(c,[stiffness]*ndof)	
	bot.set_slew_rate_proportion(c,[1.0]*ndof)
try:
	while True:
		proxy.step()
		for c in chains:
			print '---------------------------------------------'
Exemple #38
0
    def start(self):
        # ######## Setup Proxy and Components #########################
        self.proxy.start()
        self.current_first = True

        
        bot_name=m3t.get_robot_name()
        if bot_name == "":
            print 'Error: no robot components found:', bot_names
            return
        self.bot=m3.humanoid.M3Humanoid(bot_name)    
        arm_names = self.bot.get_available_chains()    
        arm_names = [x for x in arm_names if x.find('arm')!=-1]
        if len(arm_names)==0:
            print 'No arms found'
            return
        if len(arm_names)==1:
            self.arm_name=arm_names[0]
        else:
            self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0]

        # ####### Setup Hand #############
        hand_names = self.bot.get_available_chains()    
        hand_names = [x for x in hand_names if x.find('hand')!=-1]
        if len(hand_names)==0:
            print 'No hands found'
            return
        if len(arm_names)==1:
            self.hand_name=hand_names[0]
        else:
            self.hand_name = m3t.user_select_components_interactive(hand_names,single=True)[0]
        # ####### Setup Proxy #############
        self.proxy.subscribe_status(self.bot)
        self.proxy.publish_command(self.bot)
        self.proxy.make_operational_all()
        self.bot.set_motor_power_on()
        self.ndof=self.bot.get_num_dof(self.arm_name)
        
        humanoid_shm_names=self.proxy.get_available_components('m3humanoid_shm')
        if len(humanoid_shm_names) > 0:
          self.proxy.make_safe_operational(humanoid_shm_names[0])


        
        self.via_traj={}
        self.via_traj_first=True
        self.theta_curr = [0.0]*self.ndof
        # ######## Square/Circle stuff #########################
        if self.arm_name == 'right_arm':
            self.center = [0.450, -0.28, -0.1745]                
        else:
            self.center = [0.450, 0.28, -0.1745]        
        avail_chains = self.bot.get_available_chains()
        for c in avail_chains:
            if c == 'torso':
                self.center[2] += 0.5079

        self.jt = m3jt.JointTrajectory(7)
        self.axis_demo = [0, 0, 1]

        # ##### Generate square vias ############################
        '''ik_vias=[]
        length_m = 25/ 100.0
        resolution = 20
        y_left = self.center[1] + length_m/2.0
        y_right = self.center[1] - length_m/2.0
        z_top = self.center[2] + length_m/2.0
        z_bottom = self.center[2] - length_m/2.0
        dy = (y_left - y_right) / nu.float(resolution)
        dz = (z_top - z_bottom) / nu.float(resolution)
        x = self.center[0]
        if self.arm_name=='right_arm':
            # first add start point
            ik_vias.append([x, y_left, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
            # add top line
            for i in range(resolution):                
                ik_vias.append([x, y_left - (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
            # add right line
            for i in range(resolution):                
                ik_vias.append([x, y_right, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
            # add bottom line
            for i in range(resolution):                
                ik_vias.append([x, y_right + (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
            # add left line
            for i in range(resolution):                
                ik_vias.append([x, y_left, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
        else:
            # first add start point
            ik_vias.append([x, y_right, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
            # add top line
            for i in range(resolution):                
                ik_vias.append([x, y_right + (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
            # add right line
            for i in range(resolution):                
                ik_vias.append([x, y_left, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
            # add bottom line
            for i in range(resolution):                
                ik_vias.append([x, y_left - (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
            # add left line
            for i in range(resolution):                
                ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
        self.via_traj['Square']=[]
        # use zero position as reference for IK solver
        self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))
        for ikv in ik_vias:
            theta_soln = []                    
            #print 'solving for square ik via:', ikv
            if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):                    
                self.via_traj['Square'].append(theta_soln)
                self.bot.set_theta_sim_deg(self.arm_name,theta_soln)
            else:
                print 'WARNING: no IK solution found for via ', ikv
        self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))
'''
        # ##### Generate circle vias ############################
        ik_vias=[]
        diameter_m = 25.0 / 100.0
        resolution = 40
        x = self.center[0]
        for i in range(resolution*4 + 1):
            dt = 2*nu.pi/(nu.float(resolution)*4)
            t = (nu.pi/2) + i*dt
            if t > nu.pi:
                t -= 2*nu.pi
            y = self.center[1] + (diameter_m/2.0) * nu.cos(t)
            z = self.center[2] + (diameter_m/2.0) * nu.sin(t)
            ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
        self.via_traj['Circle']=[]
        # use zero position as reference for IK solver
        self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))
        for ikv in ik_vias:
            theta_soln = []                    
            if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):                    
                self.via_traj['Circle'].append(theta_soln)
                self.bot.set_theta_sim_deg(self.arm_name,theta_soln)
            else:
                print 'WARNING: no IK solution found for via ', ikv
        self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))                

        # ##### Load Via Trajectories ############################
        self.via_files={'TrajA':'kha1.via'}
        for k in self.via_files.keys():
            fn=m3t.get_animation_file(self.via_files[k])
            print "Loading:",fn
            try:
                with open(fn,'r') as f:
                    d=yaml.safe_load(f.read())
                    self.via_traj[k]=d[self.arm_name]
            except IOError:
                print 'Via file',k,'not present. Skipping...'

        # ##### Hand Trajectories ############################
        if self.hand_name is not None:
            try:
                with open(m3t.get_animation_file(self.hand_name+'_postures.yml'),'r') as f:
                    self.hand_data= yaml.safe_load(f.read())
            except Exception,e:
                print e
#You should have received a copy of the GNU Lesser General Public License
#along with M3.  If not, see <http://www.gnu.org/licenses/>.

import m3.rt_proxy as m3p
import m3.pwr as m3rt
import m3.pwr_ec as m3e
import m3.toolbox as m3t
import m3.omnibase as m3o
import time

proxy = m3p.M3RtProxy()
proxy.start()
pwr_name = proxy.get_available_components('m3pwr')
if len(pwr_name) > 1:
    pwr_name = m3t.user_select_components_interactive(pwr_name, single=True)

pwr = m3rt.M3Pwr(pwr_name[0])

proxy.subscribe_status(pwr)

base_name = proxy.get_available_components('m3omnibase')
if len(base_name) != 1:
    print 'Invalid number of base components available'
    proxy.stop()
    exit()
omni = m3o.M3OmniBase(base_name[0])
proxy.subscribe_status(omni)

scope_steer = m3t.M3ScopeN(xwidth=100, title='Steer and Total Currents')
scope_roll = m3t.M3ScopeN(xwidth=100, title='Roll Currents')
	def start(self):
		# ######## Setup Proxy and Components #########################
		self.proxy.start()
		self.current_first = True
		
		bot_name=m3t.get_robot_name()
		if bot_name == "":
			print 'Error: no robot components found:', bot_names
			return
		self.bot=m3.humanoid.M3Humanoid(bot_name)	
		arm_names = self.bot.get_available_chains()	
		arm_names = [x for x in arm_names if x.find('arm')!=-1]
		if len(arm_names)==0:
			print 'No arms found'
			return
		if len(arm_names)==1:
			self.arm_name=arm_names[0]
		else:
			self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0]

		# ####### Setup Hand #############
		hand_names=self.proxy.get_available_components('m3hand')
		hand_name=''
		if len(hand_names)>1:
			hand_name=m3t.user_select_components_interactive(chain_names,single=True)
		if len(hand_names)==1:
			hand_name=hand_names[0]
		if len(hand_name):
			self.hand=m3.hand.M3Hand(hand_name)
			self.proxy.publish_command(self.hand)
			self.proxy.subscribe_status(self.hand)
		else:
			self.hand=None
		# ####### Setup Proxy #############
		self.proxy.subscribe_status(self.bot)
		self.proxy.publish_command(self.bot)
		self.proxy.make_operational_all()
		self.bot.set_motor_power_on()
		self.ndof=self.bot.get_num_dof(self.arm_name)
		self.via_traj={}
		self.via_traj_first=True
		self.theta_curr = [0.0]*self.ndof
		# ######## Square/Circle stuff #########################
		if self.arm_name == 'right_arm':
			self.center = [0.450, -0.28, -0.1745]				
		else:
			self.center = [0.450, 0.28, -0.1745]		
		avail_chains = self.bot.get_available_chains()
		for c in avail_chains:
			if c == 'torso':
				self.center[2] += 0.5079

		self.jt = m3jt.JointTrajectory(7)
		self.axis_demo = [0, 0, 1]

		# ##### Generate square vias ############################
		ik_vias=[]
		length_m = 25/ 100.0
		resolution = 20
		y_left = self.center[1] + length_m/2.0
		y_right = self.center[1] - length_m/2.0
		z_top = self.center[2] + length_m/2.0
		z_bottom = self.center[2] - length_m/2.0
		dy = (y_left - y_right) / nu.float(resolution)
		dz = (z_top - z_bottom) / nu.float(resolution)
		x = self.center[0]
		if self.arm_name=='right_arm':
			# first add start point
			ik_vias.append([x, y_left, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
			# add top line
			for i in range(resolution):				
				ik_vias.append([x, y_left - (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
			# add right line
			for i in range(resolution):				
				ik_vias.append([x, y_right, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
			# add bottom line
			for i in range(resolution):				
				ik_vias.append([x, y_right + (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
			# add left line
			for i in range(resolution):				
				ik_vias.append([x, y_left, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
		else:
			# first add start point
			ik_vias.append([x, y_right, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
			# add top line
			for i in range(resolution):				
				ik_vias.append([x, y_right + (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
			# add right line
			for i in range(resolution):				
				ik_vias.append([x, y_left, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
			# add bottom line
			for i in range(resolution):				
				ik_vias.append([x, y_left - (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
			# add left line
			for i in range(resolution):				
				ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
		self.via_traj['Square']=[]
		# use zero position as reference for IK solver
		self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))
		for ikv in ik_vias:
			theta_soln = []					
			#print 'solving for square ik via:', ikv
			if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):					
				self.via_traj['Square'].append(theta_soln)
				self.bot.set_theta_sim_deg(self.arm_name,theta_soln)
			else:
				print 'WARNING: no IK solution found for via ', ikv
		self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))

		# ##### Generate circle vias ############################
		ik_vias=[]
		diameter_m = 25.0 / 100.0
		resolution = 20
		x = self.center[0]
		for i in range(resolution*4 + 1):
			dt = 2*nu.pi/(nu.float(resolution)*4)
			t = (nu.pi/2) + i*dt
			if t > nu.pi:
				t -= 2*nu.pi
			y = self.center[1] + (diameter_m/2.0) * nu.cos(t)
			z = self.center[2] + (diameter_m/2.0) * nu.sin(t)
			ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
		self.via_traj['Circle']=[]
		# use zero position as reference for IK solver
		self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))
		for ikv in ik_vias:
			theta_soln = []					
			if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):					
				self.via_traj['Circle'].append(theta_soln)
				self.bot.set_theta_sim_deg(self.arm_name,theta_soln)
			else:
				print 'WARNING: no IK solution found for via ', ikv
		self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))				

		# ##### Load Via Trajectories ############################
		self.via_files={'TrajA':'kha1.via'}
		pt=m3t.get_m3_animation_path()
		for k in self.via_files.keys():
			fn=pt+self.via_files[k]
			try:
				f=file(fn,'r')
				d=yaml.safe_load(f.read())
				self.via_traj[k]=d[self.arm_name]
			except IOError:
				print 'Via file',k,'not present. Skipping...'

		# ##### Hand Trajectories ############################
		if self.hand is not None:
			pf=m3t.get_m3_animation_path()+hand_name+'_postures.yml'
			f=file(pf,'r')
			self.hand_data= yaml.safe_load(f.read())
			f.close()
		self.hand_traj_first=True
		# ######## Demo and GUI #########################
		self.off=False
		self.grasp=False
		self.arm_mode_names=['Off','Zero','Current','HoldUp','Square','Circle','TrajA']
		self.hand_mode_names=['Off','Open','Grasp','Animation']
		self.arm_mode_methods=[self.step_off,self.step_zero,self.step_current,self.step_hold_up,self.step_via_traj,
				       self.step_via_traj,self.step_via_traj]
		self.hand_mode_methods=[self.step_hand_off,self.step_hand_open,self.step_hand_grasp,self.step_hand_animation]
		self.arm_mode=[0]
		self.hand_mode=[0]
		self.poses={'zero':  {'right_arm':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],'left_arm':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]},
			    'holdup':{'right_arm':[56.0, 26.0, -8.0, 84.0, 119.0, -36.0, 2.0],'left_arm':[56.0, -26.0, 8.0, 84.0, -119.0, -36.0, -2.0]}}		
		self.stiffness=[50]
		self.velocity=[25]
		self.gui.add('M3GuiModes',  'Arm Mode',      (self,'arm_mode'),range(1),[self.arm_mode_names,1],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiToggle', 'ESTOP', (self,'off'),[],[['Arm Enabled','Arm Disabled']],m3g.M3GuiWrite,column=1)
		if self.hand is not None:
			self.gui.add('M3GuiToggle', 'Grasp', (self,'grasp'),[],[['GraspOpen','GraspClosed']],m3g.M3GuiWrite,column=1)
			self.gui.add('M3GuiModes',  'Hand Mode',      (self,'hand_mode'),range(1),[self.hand_mode_names,1],m3g.M3GuiWrite,column=1)
		self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),[0],[0,100],m3g.M3GuiWrite,column=1) 
		self.gui.add('M3GuiSliders','Velocity ', (self,'velocity'),[0],[0,40],m3g.M3GuiWrite,column=1) 
		self.gui.start(self.step)
	def start(self):
		print '--------------------------'
		print 'm: Target M3 arm'
		print 'v: Target RVIZ'
		print 'b: Target Both M3 and RVIZ'
		print 'q: quit'
		print '--------------'
		print
		self.k = 'a'
		while self.k!='m' and self.k!='v' and self.k!='b' and self.k!='q':
			self.k=m3t.get_keystroke()

		if self.k=='q':
			return
		
		self.proxy = m3p.M3RtProxy()
		if self.k=='m' or self.k=='b':	
			self.proxy.start()			
			
		bot_name=m3t.get_robot_name()
		if bot_name == "":
			print 'Error: no robot components found:', bot_names
			return
		self.bot=m3f.create_component(bot_name)			
				
		arm_names = ['right_arm', 'left_arm']					
		self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0]
				
		if self.arm_name == 'right_arm':
			self.center = [0.450, -0.25, -0.1745]				
		else:
			self.center = [0.450, 0.25, -0.1745]
			
		avail_chains = self.bot.get_available_chains()
		for c in avail_chains:
			if c == 'torso':
				self.center[2] += 0.5079
			
		if self.k=='v' or self.k=='b':
			self.viz = m3v.M3Viz(self.proxy, self.bot)
			self.viz_launched = True
			self.viz.turn_sim_on()			
			
		
		if self.k=='v':			
			self.theta_0[:] = self.bot.get_theta_sim_deg(self.arm_name)[:]			
		
		if self.k=='m' or self.k=='b':
			self.proxy.subscribe_status(self.bot)
			self.proxy.publish_command(self.bot)
			self.proxy.make_operational_all()
			self.proxy.step()
			self.theta_0[:] = self.bot.get_theta_deg(self.arm_name)[:]			
			self.m3_launched = True
		
		self.theta_soln_deg = [0.]*self.bot.get_num_dof(self.arm_name)
		self.thetadot_0 = [0.]*self.bot.get_num_dof(self.arm_name)
		self.bot.set_slew_rate_proportion(self.arm_name, [1.0]*7)
		
		while True:				
			print '--------------------------'			
			print 'g: generate vias'
			print 'd: display vias'			
			print 'v: set avg velocity (Current ',self.vel_avg,')'
			print 's: set stiffness (Current',self.stiffness,')'
			if self.k=='b' or self.k=='m':
				print 'e: execute vias'
			if self.k=='b' or self.k=='v':
				print 't: test vias in visualizer'
			print 'q: quit'
			print '--------------'
			print
			m=m3t.get_keystroke()
	
			if m=='q':
				return
			
			if m=='v':
				print 'Enter avg velocity (0-60 Deg/S) [',self.vel_avg,']'
				self.vel_avg=max(0,min(60,m3t.get_float(self.vel_avg)))
				
			if m=='s':
				print 'Enter stiffness (0-1.0) [',self.stiffness,']'
				self.stiffness=max(0,min(1.0,m3t.get_float(self.stiffness)))
											
			if m == 'g':
				self.vias=[]
				print
				print '(s)quare or (c)ircle?'
				shape = None
				while shape != 's' and shape != 'c':
					shape=m3t.get_keystroke()
				length_m = 0.0
				if shape == 's':
					print
					print 'Length of square side in cm (10-25) [25]'
					length_cm = nu.float(max(10,min(25,m3t.get_int(25))))
					length_m = length_cm / 100.0
				diameter_m = 0.0
				if shape == 'c':
					print
					print 'Diameter of circle in cm (10-25) [25]'
					diameter_cm = nu.float(max(10,min(25,m3t.get_int(25))))
					diameter_m = diameter_cm / 100.0
								
				print
				print 'Enter shape resolution (1-20 vias/side) [20]'
				resolution = max(1,min(20,m3t.get_int(20)))
						
				if self.m3_launched:
					self.proxy.step()
											
				x = self.center[0]
			
				if shape == 's':
					y_left = self.center[1] + length_m/2.0
					y_right = self.center[1] - length_m/2.0
					z_top = self.center[2] + length_m/2.0
					z_bottom = self.center[2] - length_m/2.0
					dy = (y_left - y_right) / nu.float(resolution)
					dz = (z_top - z_bottom) / nu.float(resolution)
					
					if self.arm_name=='right_arm':
						# first add start point
						self.ik_vias.append([x, y_left, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
						# add top line
						for i in range(resolution):				
							self.ik_vias.append([x, y_left - (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
						# add right line
						for i in range(resolution):				
							self.ik_vias.append([x, y_right, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
						# add bottom line
						for i in range(resolution):				
							self.ik_vias.append([x, y_right + (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
						# add left line
						for i in range(resolution):				
							self.ik_vias.append([x, y_left, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
					else:
						# first add start point
						self.ik_vias.append([x, y_right, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
						# add top line
						for i in range(resolution):				
							self.ik_vias.append([x, y_right + (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
						# add right line
						for i in range(resolution):				
							self.ik_vias.append([x, y_left, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
						# add bottom line
						for i in range(resolution):				
							self.ik_vias.append([x, y_left - (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
						# add left line
						for i in range(resolution):				
							self.ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
				if shape == 'c':
					for i in range(resolution*4 + 1):
						dt = 2*nu.pi/(nu.float(resolution)*4)
						t = (nu.pi/2) + i*dt
						if t > nu.pi:
							t -= 2*nu.pi
						y = self.center[1] + (diameter_m/2.0) * nu.cos(t)
						z = self.center[2] + (diameter_m/2.0) * nu.sin(t)
						self.ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
				
						
				self.vias.append(self.theta_0[:])
				# use zero position as reference for IK solver
				ref=[0]*self.bot.get_num_dof(self.arm_name)
				# use holdup position as reference
				ref= [30,0,0,40,0,0,0]
				self.bot.set_theta_sim_deg(self.arm_name,ref)
				
				for ikv in self.ik_vias:
					theta_soln = []					
					print 'solving for ik via:', ikv
					
					if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):					
						self.vias.append(theta_soln)
						self.bot.set_theta_sim_deg(self.arm_name,theta_soln)
					else:
						print 'WARNING: no IK solution found for via ', ikv
				self.bot.set_theta_sim_deg(self.arm_name,ref)
				if self.viz_launched:					
					self.viz.step()
				self.vias.append(self.theta_0[:])
				
			if m=='d':
				print
				print '--------- IK Vias (', len(self.ik_vias), ')--------'
				print '---------------[end_xyz[3], end_axis[3]]-----------'
				for ikv  in self.ik_vias:
					print ikv
					
				print
				print '--------- Joint Vias (', len(self.vias), ')--------'
				for v  in self.vias:
					print v
				
			if m == 'e' or m=='t':
				if len(self.vias) != 0:					
					for v in self.vias:
						#print 'Adding via',v            
						self.jt.add_via_deg(v, [self.vel_avg]*self.ndof)					
					self.jt.start(self.theta_0[:], self.thetadot_0[:])
					
					print
					print '--------- Splines (', len(self.jt.splines), ')--------'
					print '------------q_0, q_f, qdot_0, qdot_f, tf--------------'					
					for s in self.jt.splines:
						print s.q_0, s.q_f, s.qdot_0, s.qdot_f, s.tf
					
					print
					print 'Hit any key to start or (q) to quit execution'
					
					
					p=m3t.get_keystroke()
					
					if p != 'q':						
						if self.m3_launched and m=='e':
							self.bot.set_motor_power_on()
							self.bot.set_mode_theta_gc(self.arm_name)
							self.bot.set_stiffness(self.arm_name, [self.stiffness]*self.bot.get_num_dof(self.arm_name))
						while not self.jt.is_splined_traj_complete():
							q = self.jt.step()
							if self.viz_launched and m=='t':
								self.bot.set_theta_sim_deg(self.arm_name, q)
								self.viz.step()
								time.sleep(0.1)
							elif self.m3_launched and m=='e':
								self.bot.set_theta_deg(self.arm_name, q)								
								self.proxy.step()
								
						self.ik_vias=[]
Exemple #42
0
proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
    print 'Error: no robot components found:', bot_name
    exit()
bot = m3f.create_component(bot_name)
proxy.publish_param(bot)  #allow to set payload
proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
bot.set_motor_power_on()
proxy.step()

chains = bot.get_available_chains()
print 'Select chains to pose'
chains = m3t.user_select_components_interactive(chains)
menu = menu_thread(bot, chains)
menu.start()

slew = {}
qdes = {}
fn = m3t.get_m3_animation_path() + m3t.get_robot_name() + '_poser_config.yml'
f = file(fn, 'r')
config = yaml.safe_load(f.read())
f.close()
stiffness = config['stiffness']
delta_thresh = config['delta_thresh']
slew_rate = config['slew_rate']
proxy.step()
dbg = ''
for c in chains:
@author: Antoine Hoarau
"""

"""
This is just a simple demo of the new velocity_gc mode, setting vel to zero"
"""

import m3.rt_proxy as m3p
import m3.humanoid as m3h

proxy = m3p.M3RtProxy()
proxy.start()

bot = m3h.M3Humanoid()
bot.initialize(proxy)
proxy.step()

import m3.toolbox as m3t
arm = m3t.user_select_components_interactive(['right_arm','left_arm'],single=True)[0]

bot.set_mode_thetadot_gc(arm)
bot.set_slew_rate_proportion(arm,[1.0]*bot.get_num_dof(arm))
bot.set_stiffness(arm,[1.0]*bot.get_num_dof(arm))
bot.set_thetadot_deg(arm,[0.0]*bot.get_num_dof(arm))
print("This will set the velocity to 0 (tries to stay still)")
raw_input("Press Enter to start")
proxy.step()
raw_input("Press Enter to stop")
proxy.stop()
exit()
def get_boards():
    print "Finding burnable board from m3_config.yml..."
    name_ec = m3t.get_ec_component_names()
    boards = {}
    for n in name_ec:
        config = m3t.get_component_config(n)
        try:
            fw = config["firmware"] + ".hex"
        except KeyError:
            print "Missing firmware key for", n, "...skipping"
            fw = None
        sn = config["ethercat"]["serial_number"]
        try:
            chid = config["chid"]
        except KeyError:
            chid = 0
        ssn = get_slave_sn()
        sid = None
        for s in ssn:
            if sn == s[0]:
                sid = s[1]
        ff = m3t.get_m3_config_path() + "eeprom/eeprom*_sn_" + str(sn) + ".hex"
        eepf = glob.glob(ff)
        if len(eepf) == 0:
            print "EEPROM file not found", ff, "skipping board..."
        if len(eepf) > 1:
            print "Multiple eeproms found for", ff
            print "Select correct eeprom ID"
            for i in range(len(eepf)):
                print i, " : ", eepf[i]
            idx = m3t.get_int()
            eepf = [eepf[idx]]
        if sid != None and len(eepf) == 1 and fw is not None and chid == 0:
            boards[n] = {
                "firmware": fw,
                "sn": sn,
                "slave_id": sid,
                "eeprom": eepf[0],
                "connected": False,
                "write_success": False,
            }

    print "Found the following valid configurations: "
    print boards.keys()
    print
    print "Selection type"
    print "-----------------"
    print "a: all boards"
    print "s: single board"
    print "m: multiple boards"
    t = raw_input()
    if t != "a" and t != "s" and t != "m":
        print "Invalid selection"
        return {}
    rem = []
    if t == "m":
        for k in boards.keys():
            print "-----------------------"
            print "Burn ", k, "board [y]?"
            if not m3t.get_yes_no("y"):
                rem.append(k)
        for r in rem:
            boards.pop(r)
    if t == "s":
        k = m3t.user_select_components_interactive(boards.keys(), single=True)
        if len(k) == 0:
            return {}
        boards = {k[0]: boards[k[0]]}

    print "-------------------------------"
    print "Burning the following boards: "
    for k in boards.keys():
        print k
    return boards
# ######################################################
proxy = m3p.M3RtProxy()
proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
    print 'Error: no robot components found:', bot_name
bot = m3f.create_component(bot_name)
proxy.publish_param(bot)  #allow to set payload
proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
bot.set_motor_power_on()
chains = bot.get_available_chains()
print 'Select chain'
chains = m3t.user_select_components_interactive(chains, single=True)
for c in chains:
    ndof = bot.get_num_dof(c)
    bot.set_mode_theta_gc(c)
    bot.set_theta_deg(c, [0.0] * ndof)
    bot.set_stiffness(c, [0.0] * ndof)
try:
    while True:
        proxy.step()
        for c in chains:
            print '---------------------------------------------'
            print 'Chain: ', c
            print 'Tool Position: (m)', bot.get_tool_position(c)
            print 'Theta (Deg): ', bot.get_theta_deg(c)
            print 'Tool Velocity (m/S)', bot.get_tool_velocity(c)
        time.sleep(0.1)
    gripper.set_mode_torque()
    gripper.set_force_g([a,b])
    proxy.step()
    for i in range(n):
        proxy.step()
        print 'Force (g)',gripper.get_force_g()
        print 'Position (mm)',gripper.get_pos_mm()
        time.sleep(0.2)
    
proxy = m3p.M3RtProxy()
proxy.start()
proxy.make_operational_all()

#Setup Components
chain_names=proxy.get_available_components('m3gripper')
gripper_name=m3t.user_select_components_interactive(chain_names,single=True)
gripper=m3h.M3Gripper(gripper_name[0])
proxy.publish_command(gripper)
proxy.subscribe_status(gripper)
if len(proxy.get_available_components('m3pwr')):
    pwr_name=proxy.get_available_components('m3pwr')[0]
    pwr=m3f.create_component(pwr_name)
    proxy.publish_command(pwr)
    pwr.set_motor_power_on()
      
while True:
    proxy.step()
    print '--------------'
    print 'f: grip'
    print 'o: open'
    print 'd: demo'
#CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#POSSIBILITY OF SUCH DAMAGE.

import m3.toolbox as m3t
import m3.toolbox_ctrl as m3tc
import time




proxy = m3p.M3RtProxy()
		
proxy.start()
name_ec=m3t.user_select_components_interactive(proxy.get_available_components('m3actuator_ec'),single=True)
if name_ec is None:
	exit()
comp=m3f.create_component(name_ec)
proxy.subscribe_status(comp)
proxy.make_operational(name_ec)
q_log=[]
tq_log=[]
print 'Ready to generate motion? Hit any key to start'
m3t.get_keystroke()
ts=time.time()
try:
	while time.time()-ts>5.0:
		proxy.step()
		q=comp.status.qei_on
		tq=comp.status.adc_torque
	def start(self):

		self.proxy.start()
		cnames=self.proxy.get_available_components('m3actuator_ec')
		self.names=m3t.user_select_components_interactive(cnames)
		if len(self.names)==0:
			return
		self.actuator_ec=[]
		for name in self.names:
			self.actuator_ec.append(m3f.create_component(name))
			self.proxy.subscribe_status(self.actuator_ec[-1])
			self.proxy.publish_command(self.actuator_ec[-1]) 
			self.proxy.publish_param(self.actuator_ec[-1]) 
			self.proxy.make_operational(name)
		
		#pwr_ec=self.proxy.get_available_components('m3pwr_ec')
		#pwr_rt=self.proxy.get_available_components('m3pwr')
		#print 'A',pwr_rt[0],pwr_ec[0]
		#if len(pwr_rt):
			#pr=m3f.create_component(pwr_rt[0])
			#self.proxy.publish_command(pr)
			#self.proxy.make_operational(pwr_rt[0])
			#self.proxy.make_operational(pwr_ec[0])
			#pr.set_motor_power_on()
			
		pwr_rt=m3t.get_actuator_ec_pwr_component_name(self.names[0])
		pwr_ec=pwr_rt.replace('m3pwr','m3pwr_ec')
		pr=m3f.create_component(pwr_rt)
		self.proxy.publish_command(pr)
		self.proxy.make_operational(pwr_rt)
		self.proxy.make_operational(pwr_ec)
		self.proxy.subscribe_status(pr)
		pr.set_motor_power_on()
		
		tmax=[x.param.t_max for x in self.actuator_ec]
		tmin=[x.param.t_min for x in self.actuator_ec]
		
		
		self.proxy.step()
		for c in self.actuator_ec:
			self.bias.append(c.status.adc_torque)
		tl=min(tmin)-self.bias[0]
		tu=max(tmax)-self.bias[0]
		
		#Create gui
		self.mode=[0]
		self.pwm_desire=[0]
		self.current_desire=[0]

		self.save=False
		self.save_last=False
		self.do_scope=False
		self.scope = None
		
		self.scope_keys=m3t.get_msg_fields(self.actuator_ec[0].status)
		self.scope_keys.sort()
		self.scope_keys=['None']+self.scope_keys
		self.scope_field1=[0]
		self.scope_field2=[0]

		self.status_dict=self.proxy.get_status_dict()
		self.param_dict=self.proxy.get_param_dict()
		self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
		self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
		self.gui.add('M3GuiModes',  'Mode',      (self,'mode'),range(len(self.actuator_ec)),[['off','pwm','torque','current','brake'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','pwmDesire', (self,'pwm_desire'),range(len(self.actuator_ec)),[-1000,1000],m3g.M3GuiWrite) 
		self.gui.add('M3GuiSliders','currentDesire', (self,'current_desire'),range(len(self.actuator_ec)),[-10000,10000],m3g.M3GuiWrite) 
		self.gui.add('M3GuiToggle', 'Save',      (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.add('M3GuiToggle', 'Scope',      (self,'do_scope'),[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'Scope1',	(self,'scope_field1'),range(1),[self.scope_keys,1],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'Scope2',	(self,'scope_field2'),range(1),[self.scope_keys,1],m3g.M3GuiWrite)
	
		self.gui.start(self.step)
    def start(self):
        self.proxy.start()
        #print 'Enable RVIZ [n]?'
        self.rviz = False
        #if m3t.get_yes_no('n'):
        #	self.rviz = True

        chain_names = self.proxy.get_chain_components()
        self.chain = []
        if len(chain_names) > 0:
            print 'Select kinematic chain'
            self.chain_names = m3t.user_select_components_interactive(
                chain_names, single=True)
        self.chain.append(m3f.create_component(self.chain_names[0]))
        self.proxy.subscribe_status(self.chain[-1])
        self.limb = m3t.get_chain_limb_name(self.chain_names[0])

        joint_names = m3t.get_chain_joint_names(self.chain_names[0])
        print 'Select joint'
        joint_names = m3t.user_select_components_interactive(joint_names,
                                                             single=True)
        self.joint = []
        self.actuator = []
        self.actuator_ec = []
        acutator_names = []
        for n in joint_names:
            self.joint.append(m3f.create_component(n))
            actuator_name = m3t.get_joint_actuator_component_name(n)
            actuator_ec_name = m3t.get_actuator_ec_component_name(
                actuator_name)
            self.actuator.append(m3f.create_component(actuator_name))
            self.actuator_ec.append(m3f.create_component(actuator_ec_name))
            self.proxy.subscribe_status(self.joint[-1])
            self.proxy.publish_param(self.joint[-1])
            self.proxy.subscribe_status(self.actuator[-1])
            self.proxy.publish_param(self.actuator[-1])
            if self.actuator_ec[0] is not None:
                self.proxy.subscribe_status(self.actuator_ec[-1])
                self.proxy.publish_param(self.actuator_ec[-1])

        #kine_names=self.proxy.get_available_components('m3dynamatics')
        self.kine = []
        #if len(kine_names)>0:
        #print 'Select dynamatics controller'
        #kine_names=m3t.user_select_components_interactive(kine_names)

        #for n in kine_names:
        #self.kine.append(m3f.create_component(n))
        #self.proxy.subscribe_status(self.kine[-1])

        bot_name = m3t.get_robot_name()
        if bot_name == "":
            print 'Error: no robot components found:', bot_names
            return
        self.bot = m3f.create_component(bot_name)

        if self.rviz:
            self.viz = m3v.M3Viz(self.proxy, self.bot)

        #self.proxy.publish_param(self.bot) #allow to set payload
        #self.proxy.subscribe_status(self.bot)
        #self.proxy.publish_command(self.bot)
        #self.proxy.make_operational_all()
        self.bot.initialize(self.proxy)
        #self.chain_names = self.bot.get_available_chains()
        #Create gui
        self.mode = [0]
        self.posture = [0]
        self.theta_desire_a = [0] * self.bot.get_num_dof(self.limb)
        self.theta_desire_b = [0] * self.bot.get_num_dof(self.limb)
        self.stiffness = [50.0] * self.bot.get_num_dof(self.limb)
        self.thetadot = [10.0] * self.bot.get_num_dof(self.limb)

        #print 'Selected: ',self.chain_names[0],self.limb,self.bot.get_num_dof(self.limb)

        #self.slew=[0]
        self.save = False
        self.save_last = False
        self.status_dict = self.proxy.get_status_dict()
        self.param_dict = self.proxy.get_param_dict()
        self.gui.add('M3GuiModes', 'Mode', (self, 'mode'), range(1), [[
            'Off', 'Pwm', 'Torque', 'Theta', 'Torque_GC', 'Theta_GC',
            'Theta_MJ', 'Theta_GC_MJ'
        ], 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiTree',
                     'Status', (self, 'status_dict'), [], [],
                     m3g.M3GuiRead,
                     column=1)
        self.gui.add('M3GuiTree',
                     'Param', (self, 'param_dict'), [], [],
                     m3g.M3GuiWrite,
                     column=1)

        self.gui.add('M3GuiSliders',
                     'ThetaA (Deg)', (self, 'theta_desire_a'),
                     range(len(self.theta_desire_a)), [-45, 140],
                     m3g.M3GuiWrite,
                     column=2)
        self.gui.add('M3GuiModes',
                     'Posture', (self, 'posture'),
                     range(1), [['A', 'B', 'Cycle'], 1],
                     m3g.M3GuiWrite,
                     column=2)
        self.gui.add('M3GuiSliders',
                     'ThetaB (Deg)', (self, 'theta_desire_b'),
                     range(len(self.theta_desire_b)), [-45, 140],
                     m3g.M3GuiWrite,
                     column=2)

        self.gui.add('M3GuiSliders',
                     'Stiffness ', (self, 'stiffness'),
                     range(len(self.stiffness)), [0, 100],
                     m3g.M3GuiWrite,
                     column=3)
        self.gui.add('M3GuiToggle',
                     'Save', (self, 'save'), [], [['On', 'Off']],
                     m3g.M3GuiWrite,
                     column=3)
        self.gui.add('M3GuiSliders',
                     'ThetaDot ', (self, 'thetadot'),
                     range(len(self.thetadot)), [0, 100],
                     m3g.M3GuiWrite,
                     column=3)

        #self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.sea_joint)),[0,100],m3g.M3GuiWrite,column=3)

        self.gui.start(self.step)
    def start(self):
        # ######## Setup Proxy and Components #########################
        self.proxy.start()
        self.current_first = True

        bot_name = m3t.get_robot_name()
        if bot_name == "":
            print 'Error: no robot components found:', bot_names
            return
        self.bot = m3.humanoid.M3Humanoid(bot_name)
        arm_names = self.bot.get_available_chains()
        arm_names = [x for x in arm_names if x.find('arm') != -1]
        if len(arm_names) == 0:
            print 'No arms found'
            return
        if len(arm_names) == 1:
            self.arm_name = arm_names[0]
        else:
            self.arm_name = m3t.user_select_components_interactive(
                arm_names, single=True)[0]

        self.jnts = self.bot.get_joint_names(self.arm_name)

        comp = {}

        for c in self.jnts:
            comp[c] = {
                'comp_rt': None,
                'comp_j': None,
                'torque_act': [],
                'torque_joint': [],
                'torque_gravity': [],
                'is_wrist': False
            }
            if (c.find('j5') >= 0 or c.find('j6') >= 0):
                comp[c]['is_wrist'] = True

        for c in self.jnts:
            comp[c]['comp_j'] = mcf.create_component(c)
            comp[c]['comp_rt'] = mcf.create_component(
                c.replace('joint', 'actuator'))
            self.proxy.subscribe_status(comp[c]['comp_rt'])
            self.proxy.subscribe_status(comp[c]['comp_j'])

        # ####### Setup Proxy #############
        self.proxy.subscribe_status(self.bot)
        self.proxy.publish_command(self.bot)
        self.proxy.make_operational_all()
        self.bot.set_motor_power_on()
        self.ndof = self.bot.get_num_dof(self.arm_name)

        humanoid_shm_names = self.proxy.get_available_components(
            'm3humanoid_shm')
        if len(humanoid_shm_names) > 0:
            self.proxy.make_safe_operational(humanoid_shm_names[0])

        self.bot.set_mode_off(self.arm_name)

        print 'This script will calibrate the zero-torque of the A2 arm while posed using current torque_gravity estimate'
        print '----------------------------------------------------------------------------------------------------------'
        print 'With E-Stop down, pose the arm in desired position to calibrate torque zeroes around.'
        print 'Press any key when ready posed.'

        raw_input()

        time.sleep(0.5)

        self.proxy.step()

        self.theta_curr = self.bot.get_theta_deg(self.arm_name)[:]

        self.proxy.step()

        print 'Posed position set.'
        print 'Release E-stop and press any key for arm to hold pose.'

        raw_input()

        self.bot.set_mode_theta_gc(self.arm_name)
        self.bot.set_theta_deg(self.arm_name, self.theta_curr)
        self.bot.set_stiffness(self.arm_name, [1.0] * 7)
        self.bot.set_slew_rate_proportion(self.arm_name, [1.0] * self.ndof)

        self.proxy.step()

        print 'Press any key to start torque calibration for all joints.'

        raw_input()

        self.proxy.step()

        # ###########################
        ns = 30
        for i in range(ns):
            self.proxy.step()
            print '---------'
            for c in comp.keys():
                tqj = comp[c]['comp_j'].get_torque_mNm()
                tqg = comp[c]['comp_j'].get_torque_gravity_mNm() / 1000.0
                tqa = comp[c]['comp_rt'].get_torque_mNm()
                comp[c]['torque_act'].append(tqa)
                comp[c]['torque_joint'].append(tqj)
                comp[c]['torque_gravity'].append(tqg)
                if comp[c]['is_wrist']:
                    print c, ':joint', tqj, ':gravity', tqg, ':actuator', tqa
                else:
                    print c, ':joint', tqj, ':gravity', tqg,
            time.sleep(0.05)

        do_query = True

        # ###########################
        for c in comp.keys():
            print '--------', c, '---------'
            tqg = float(na.array(comp[c]['torque_gravity'], na.Float32).mean())
            tqj = float(na.array(comp[c]['torque_joint'], na.Float32).mean())
            tqa = float(na.array(comp[c]['torque_act'], na.Float32).mean())
            if not comp[c]['is_wrist']:
                bias = tqa + tqg
                torque = M3TorqueSensor(
                    comp[c]['comp_rt'].config['calib']['torque']['type'])
                print 'Measured torque:', tqa, 'Torque gravity:', tqg
                print 'Delta of', bias, 'mNm'
                comp[c]['comp_rt'].config['calib']['torque']['cb_bias'] = comp[
                    c]['comp_rt'].config['calib']['torque']['cb_bias'] - bias
                comp[c]['comp_rt'].config['calibration_date'] = time.asctime()
                if do_query:
                    print 'Save calibration? [y]'
                    if m3t.get_yes_no('y'):
                        comp[c]['comp_rt'].write_config()
                else:
                    comp[c]['comp_rt'].write_config()
            else:
                print 'Wrist joint...'
                if c.find('j5') != -1:  #do j5/j6 at once
                    cc = None
                    for x in comp.keys():
                        if x.find('j6') != -1:
                            cc = x
                    if cc is None:
                        print 'Did not find coupled joint to', c
                    tqg_c = float(
                        na.array(comp[cc]['torque_gravity'],
                                 na.Float32).mean())
                    tqj_c = float(
                        na.array(comp[cc]['torque_joint'], na.Float32).mean())
                    tqa_c = float(
                        na.array(comp[cc]['torque_act'], na.Float32).mean())
                    x = comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][
                        0]  #Joint to actuator matrix
                    y = comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][
                        1]
                    m = comp[cc]['comp_j'].config['transmission'][
                        'tqj_to_tqa'][0]
                    n = comp[cc]['comp_j'].config['transmission'][
                        'tqj_to_tqa'][1]
                    tqg_a5 = x * tqg + y * tqg_c
                    tqg_a6 = m * tqg_c + n * tqg
                    bias_5 = tqa + tqg_a5
                    bias_6 = tqa_c + tqg_a6
                    torque_5 = M3TorqueSensor(
                        comp[c]['comp_rt'].config['calib']['torque']['type'])
                    torque_6 = M3TorqueSensor(
                        comp[cc]['comp_rt'].config['calib']['torque']['type'])
                    print '------------'
                    print 'J5: Previous joint torque', tqj, 'with joint torque gravity', tqg
                    print 'J5: Previous actuator torque', tqa, 'with actuator torque gravity', tqg_a5
                    print 'J5: Actuator delta of', bias_5, 'mNm'
                    print '------------'
                    print 'J6: Previous joint torque', tqj_c, 'with joint torque gravity', tqg_c
                    print 'J6: Previous actuator torque', tqa_c, 'with actuator torque gravity', tqg_a6
                    print 'J6: Actuator delta of', bias_6, 'mNm'
                    print '------------'
                    comp[c]['comp_rt'].config['calib']['torque'][
                        'cb_bias'] = comp[c]['comp_rt'].config['calib'][
                            'torque']['cb_bias'] - bias_5
                    comp[c]['comp_rt'].config[
                        'calibration_date'] = time.asctime()
                    comp[cc]['comp_rt'].config['calib']['torque'][
                        'cb_bias'] = comp[cc]['comp_rt'].config['calib'][
                            'torque']['cb_bias'] - bias_6
                    comp[cc]['comp_rt'].config[
                        'calibration_date'] = time.asctime()
                    if do_query:
                        print 'Save calibration? [y]'
                        if m3t.get_yes_no('y'):
                            comp[c]['comp_rt'].write_config()
                            comp[cc]['comp_rt'].write_config()
                    else:
                        comp[c]['comp_rt'].write_config()
                        comp[cc]['comp_rt'].write_config()

        self.bot.set_mode_off(self.arm_name)
        self.proxy.stop()
  proxy.make_safe_operational(humanoid_shm_names[0])


if not rviz == True:
    bot.set_motor_power_on()

proxy.step()
if rviz == True:
    viz.step()

stiffness=0.5
step_delta=.002 #meters

print 'Select arm:'		
arm_names = ['right_arm', 'left_arm']		
arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0]

while True:
    proxy.step()
    print '--------------'
    print 's: set stiffness (Current',stiffness,')'
    print 'd: set step delta (Current',step_delta,'(m))'
    print 'e: execute ijkt controller'
    print 'q: quit'
    print '--------------'
    print
    k=m3t.get_keystroke()
    if k=='q':
	break
    if k=='s':
	print 'Enter stiffness (0-1.0) [',stiffness,']'
Exemple #52
0
    def start(self):
        print '--------------------------'
        print 'm: Target M3 arm'
        print 'v: Target RVIZ'
        print 'b: Target Both M3 and RVIZ'
        print 'q: quit'
        print '--------------'
        print
        self.k = 'a'
        while self.k != 'm' and self.k != 'v' and self.k != 'b' and self.k != 'q':
            self.k = m3t.get_keystroke()

        if self.k == 'q':
            return

        self.proxy = m3p.M3RtProxy()
        if self.k == 'm' or self.k == 'b':
            self.proxy.start()

        bot_name = m3t.get_robot_name()
        if bot_name == "":
            print 'Error: no robot components found:', bot_names
            return
        self.bot = m3f.create_component(bot_name)

        arm_names = ['right_arm', 'left_arm']
        self.arm_name = m3t.user_select_components_interactive(arm_names,
                                                               single=True)[0]

        if self.arm_name == 'right_arm':
            self.center = [0.450, -0.25, -0.1745]
        else:
            self.center = [0.450, 0.25, -0.1745]

        avail_chains = self.bot.get_available_chains()
        for c in avail_chains:
            if c == 'torso':
                self.center[2] += 0.5079

        if self.k == 'v' or self.k == 'b':
            self.viz = m3v.M3Viz(self.proxy, self.bot)
            self.viz_launched = True
            self.viz.turn_sim_on()

        if self.k == 'v':
            self.theta_0[:] = self.bot.get_theta_sim_deg(self.arm_name)[:]

        if self.k == 'm' or self.k == 'b':
            self.proxy.subscribe_status(self.bot)
            self.proxy.publish_command(self.bot)
            self.proxy.make_operational_all()
            self.proxy.step()
            self.theta_0[:] = self.bot.get_theta_deg(self.arm_name)[:]
            self.m3_launched = True

        self.theta_soln_deg = [0.] * self.bot.get_num_dof(self.arm_name)
        self.thetadot_0 = [0.] * self.bot.get_num_dof(self.arm_name)
        self.bot.set_slew_rate_proportion(self.arm_name, [1.0] * 7)

        while True:
            print '--------------------------'
            print 'g: generate vias'
            print 'd: display vias'
            print 'v: set avg velocity (Current ', self.vel_avg, ')'
            print 's: set stiffness (Current', self.stiffness, ')'
            if self.k == 'b' or self.k == 'm':
                print 'e: execute vias'
            if self.k == 'b' or self.k == 'v':
                print 't: test vias in visualizer'
            print 'q: quit'
            print '--------------'
            print
            m = m3t.get_keystroke()

            if m == 'q':
                return

            if m == 'v':
                print 'Enter avg velocity (0-60 Deg/S) [', self.vel_avg, ']'
                self.vel_avg = max(0, min(60, m3t.get_float(self.vel_avg)))

            if m == 's':
                print 'Enter stiffness (0-1.0) [', self.stiffness, ']'
                self.stiffness = max(0, min(1.0,
                                            m3t.get_float(self.stiffness)))

            if m == 'g':
                self.vias = []
                print
                print '(s)quare or (c)ircle?'
                shape = None
                while shape != 's' and shape != 'c':
                    shape = m3t.get_keystroke()
                length_m = 0.0
                if shape == 's':
                    print
                    print 'Length of square side in cm (10-25) [25]'
                    length_cm = nu.float(max(10, min(25, m3t.get_int(25))))
                    length_m = length_cm / 100.0
                diameter_m = 0.0
                if shape == 'c':
                    print
                    print 'Diameter of circle in cm (10-25) [25]'
                    diameter_cm = nu.float(max(10, min(25, m3t.get_int(25))))
                    diameter_m = diameter_cm / 100.0

                print
                print 'Enter shape resolution (1-20 vias/side) [20]'
                resolution = max(1, min(20, m3t.get_int(20)))

                if self.m3_launched:
                    self.proxy.step()

                x = self.center[0]

                if shape == 's':
                    y_left = self.center[1] + length_m / 2.0
                    y_right = self.center[1] - length_m / 2.0
                    z_top = self.center[2] + length_m / 2.0
                    z_bottom = self.center[2] - length_m / 2.0
                    dy = (y_left - y_right) / nu.float(resolution)
                    dz = (z_top - z_bottom) / nu.float(resolution)

                    if self.arm_name == 'right_arm':
                        # first add start point
                        self.ik_vias.append([
                            x, y_left, z_top, self.axis_demo[0],
                            self.axis_demo[1], self.axis_demo[2]
                        ])
                        # add top line
                        for i in range(resolution):
                            self.ik_vias.append([
                                x, y_left - (i + 1) * dy, z_top,
                                self.axis_demo[0], self.axis_demo[1],
                                self.axis_demo[2]
                            ])
                        # add right line
                        for i in range(resolution):
                            self.ik_vias.append([
                                x, y_right, z_top - (i + 1) * dz,
                                self.axis_demo[0], self.axis_demo[1],
                                self.axis_demo[2]
                            ])
                        # add bottom line
                        for i in range(resolution):
                            self.ik_vias.append([
                                x, y_right + (i + 1) * dy, z_bottom,
                                self.axis_demo[0], self.axis_demo[1],
                                self.axis_demo[2]
                            ])
                        # add left line
                        for i in range(resolution):
                            self.ik_vias.append([
                                x, y_left, z_bottom + (i + 1) * dz,
                                self.axis_demo[0], self.axis_demo[1],
                                self.axis_demo[2]
                            ])
                    else:
                        # first add start point
                        self.ik_vias.append([
                            x, y_right, z_top, self.axis_demo[0],
                            self.axis_demo[1], self.axis_demo[2]
                        ])
                        # add top line
                        for i in range(resolution):
                            self.ik_vias.append([
                                x, y_right + (i + 1) * dy, z_top,
                                self.axis_demo[0], self.axis_demo[1],
                                self.axis_demo[2]
                            ])
                        # add right line
                        for i in range(resolution):
                            self.ik_vias.append([
                                x, y_left, z_top - (i + 1) * dz,
                                self.axis_demo[0], self.axis_demo[1],
                                self.axis_demo[2]
                            ])
                        # add bottom line
                        for i in range(resolution):
                            self.ik_vias.append([
                                x, y_left - (i + 1) * dy, z_bottom,
                                self.axis_demo[0], self.axis_demo[1],
                                self.axis_demo[2]
                            ])
                        # add left line
                        for i in range(resolution):
                            self.ik_vias.append([
                                x, y_right, z_bottom + (i + 1) * dz,
                                self.axis_demo[0], self.axis_demo[1],
                                self.axis_demo[2]
                            ])
                if shape == 'c':
                    for i in range(resolution * 4 + 1):
                        dt = 2 * nu.pi / (nu.float(resolution) * 4)
                        t = (nu.pi / 2) + i * dt
                        if t > nu.pi:
                            t -= 2 * nu.pi
                        y = self.center[1] + (diameter_m / 2.0) * nu.cos(t)
                        z = self.center[2] + (diameter_m / 2.0) * nu.sin(t)
                        self.ik_vias.append([
                            x, y, z, self.axis_demo[0], self.axis_demo[1],
                            self.axis_demo[2]
                        ])

                self.vias.append(self.theta_0[:])
                # use zero position as reference for IK solver
                ref = [0] * self.bot.get_num_dof(self.arm_name)
                # use holdup position as reference
                ref = [30, 0, 0, 40, 0, 0, 0]
                self.bot.set_theta_sim_deg(self.arm_name, ref)

                for ikv in self.ik_vias:
                    theta_soln = []
                    print 'solving for ik via:', ikv

                    if self.bot.get_tool_axis_2_theta_deg_sim(
                            self.arm_name, ikv[:3], ikv[3:], theta_soln):
                        self.vias.append(theta_soln)
                        self.bot.set_theta_sim_deg(self.arm_name, theta_soln)
                    else:
                        print 'WARNING: no IK solution found for via ', ikv
                self.bot.set_theta_sim_deg(self.arm_name, ref)
                if self.viz_launched:
                    self.viz.step()
                self.vias.append(self.theta_0[:])

            if m == 'd':
                print
                print '--------- IK Vias (', len(self.ik_vias), ')--------'
                print '---------------[end_xyz[3], end_axis[3]]-----------'
                for ikv in self.ik_vias:
                    print ikv

                print
                print '--------- Joint Vias (', len(self.vias), ')--------'
                for v in self.vias:
                    print v

            if m == 'e' or m == 't':
                if len(self.vias) != 0:
                    for v in self.vias:
                        #print 'Adding via',v
                        self.jt.add_via_deg(v, [self.vel_avg] * self.ndof)
                    self.jt.start(self.theta_0[:], self.thetadot_0[:])

                    print
                    print '--------- Splines (', len(
                        self.jt.splines), ')--------'
                    print '------------q_0, q_f, qdot_0, qdot_f, tf--------------'
                    for s in self.jt.splines:
                        print s.q_0, s.q_f, s.qdot_0, s.qdot_f, s.tf

                    print
                    print 'Hit any key to start or (q) to quit execution'

                    p = m3t.get_keystroke()

                    if p != 'q':
                        if self.m3_launched and m == 'e':
                            self.bot.set_motor_power_on()
                            self.bot.set_mode_theta_gc(self.arm_name)
                            self.bot.set_stiffness(
                                self.arm_name, [self.stiffness] *
                                self.bot.get_num_dof(self.arm_name))
                        while not self.jt.is_splined_traj_complete():
                            q = self.jt.step()
                            if self.viz_launched and m == 't':
                                self.bot.set_theta_sim_deg(self.arm_name, q)
                                self.viz.step()
                                time.sleep(0.1)
                            elif self.m3_launched and m == 'e':
                                self.bot.set_theta_deg(self.arm_name, q)
                                self.proxy.step()

                        self.ik_vias = []