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)
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