示例#1
0
    def __init__(self, bot, hand_name, gui=None, gui_column=1):
        self.name = hand_name
        self.gui = gui
        self.bot = bot
        self.gui_col = gui_column

        self.data = {}
        self.data['param'] = {}
        self.param = self.data['param']
        self.ndof = self.bot.get_num_dof(self.name)
        try:
            with open(m3t.get_animation_file(self.name + '_postures.yml'),
                      'r') as f:
                self.data = yaml.safe_load(f.read())
                self.param = self.data['param']
                print 'Loaded animation:', self.data
        except:
            print "Animation file not found, using default anim"
            self.param['stiffness'] = [1.0] * self.ndof
            self.param['q_slew_rate'] = [1.0] * self.ndof
            self.param['grasp_torque'] = [200] * self.ndof
            self.data['postures'].append([0.0] * self.ndof)
            self.data['postures'].append([250.0] * self.ndof)

        #Create gui
        self.theta_desire = [0, 0, 0, 0, 0]
        self.mode = [1, 1, 1, 1, 1]
        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()

        if self.gui:
            self.gui.add('M3GuiTree',
                         'Param', (self, 'param'), [], [],
                         m3g.M3GuiWrite,
                         column=self.gui_col)
            self.gui.add('M3GuiToggle', 'Animation', (self, 'run'), [],
                         [['Run', 'Stop']], m3g.M3GuiWrite, self.gui_col)
            self.gui.add('M3GuiModes', 'Joint', (self, 'mode'), range(5),
                         [['Off', 'Enabled'], 1], m3g.M3GuiWrite, self.gui_col)
            self.gui.add('M3GuiSliders', 'Theta (Deg)', (self, 'theta_desire'),
                         range(5), [0, 300], m3g.M3GuiWrite, self.gui_col)
            self.gui.add('M3GuiToggle', 'Power Grasp', (self, 'grasp'), [],
                         [['Run', 'Stop']], m3g.M3GuiWrite, self.gui_col)
    def __init__(self,bot,hand_name,gui=None,gui_column=1):
        self.name = hand_name
        self.gui = gui
        self.bot=bot
        self.gui_col = gui_column

        self.data={}
        self.data['param']={}
        self.param = self.data['param']
        self.ndof = self.bot.get_num_dof(self.name)
        try:
            with open(m3t.get_animation_file(self.name+'_postures.yml'),'r') as f:
                self.data= yaml.safe_load(f.read())
                self.param=self.data['param']
                print 'Loaded animation:',self.data
        except:
            print "Animation file not found, using default anim"
            self.param['stiffness']=[1.0]*self.ndof
            self.param['q_slew_rate']=[1.0]*self.ndof
            self.param['grasp_torque']=[200]*self.ndof
            self.data['postures'].append([0.0]*self.ndof)            
            self.data['postures'].append([250.0]*self.ndof)
            
        #Create gui
        self.theta_desire=[0,0,0,0,0]
        self.mode=[1,1,1,1,1]
        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()
        
        if self.gui:
            self.gui.add('M3GuiTree',   'Param',   (self,'param'),[],[],m3g.M3GuiWrite,column=self.gui_col)
            self.gui.add('M3GuiToggle', 'Animation',      (self,'run'),[],[['Run','Stop']],m3g.M3GuiWrite,self.gui_col)
            self.gui.add('M3GuiModes',  'Joint',      (self,'mode'),range(5),[['Off','Enabled'],1],m3g.M3GuiWrite,self.gui_col)
            self.gui.add('M3GuiSliders','Theta (Deg)', (self,'theta_desire'),range(5),[0,300],m3g.M3GuiWrite,self.gui_col)
            self.gui.add('M3GuiToggle', 'Power Grasp', (self,'grasp'),[],[['Run','Stop']],m3g.M3GuiWrite,self.gui_col)
示例#3
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