def load_postures(): fn=m3t.get_m3_animation_path()+'demo_head_h1_postures.yml' try: f=file(fn,'r') postures = yaml.safe_load(f.read()) f.close() except IOError: print 'Posture file not present'
def run(self): vias = {} for c in self.chains: vias[c] = [] while not self.done: print '--------------' print 'r: record via' print 's: save via file' print 'p: print current pose' print 'q: quit' print '--------------' print k = m3t.get_keystroke() print 'Dbg', dbg if k == 'r': print '-------------' for c in self.chains: vias[c].append(m3t.float_list(self.bot.get_theta_deg(c))) print 'Record of: ', vias[c][-1], 'for', c if k == 'p': print '----------------' for c in self.chains: print 'Chain:', c, ' : ', self.bot.get_theta_deg(c) print '----------------' if k == 's': bot_name = m3t.get_robot_name() fn = bot_name + '_' + m3t.time_string() print 'Enter via file name [', fn, ']' fn = m3t.get_string(fn) fn = m3t.get_m3_animation_path() + fn + '.via' print 'Writing file: ', fn f = file(fn, 'w') d = {} for c in self.chains: ndof = self.bot.get_num_dof(c) if c == 'torso': param = { 'slew': [1.0] * ndof, 'stiffness': [0.8] * ndof, 'velocity': [25.0, 15.0] } else: param = { 'slew': [1.0] * ndof, 'stiffness': [0.4] * ndof, 'velocity': [25.0] * ndof } d[c] = { 'postures': vias[c], 'param': param } #safe defaults f.write(yaml.safe_dump(d)) vias = {} for c in self.chains: vias[c] = [] if k == 'q': self.done = True
def load_animation(self, anim_name): """Call with each animation to load at startup""" ani_paths = m3t.get_m3_animation_path() for ani_path in ani_paths: filename = ani_path + "mouth/" + anim_name + ".yml" try: f=file(filename,'r') self.animations[anim_name]= yaml.safe_load(f.read()) except (IOError, EOFError): print 'Animation file not present:',filename for k in self.animations[anim_name]['sequence']: filename = ani_path + "/mouth/" + k['file'] try: k['image']= Image.open(filename) except (IOError, EOFError): print 'File',filename,'not found'
def run(self): vias = {} for c in self.chains: vias[c] = [] while not self.done: print "--------------" print "r: record via" print "s: save via file" print "p: print current pose" print "q: quit" print "--------------" print k = m3t.get_keystroke() print "Dbg", dbg if k == "r": print "-------------" for c in self.chains: vias[c].append(m3t.float_list(self.bot.get_theta_deg(c))) print "Record of: ", vias[c][-1], "for", c if k == "p": print "----------------" for c in self.chains: print "Chain:", c, " : ", self.bot.get_theta_deg(c) print "----------------" if k == "s": bot_name = m3t.get_robot_name() fn = bot_name + "_" + m3t.time_string() print "Enter via file name [", fn, "]" fn = m3t.get_string(fn) fn = m3t.get_m3_animation_path() + fn + ".via" print "Writing file: ", fn f = file(fn, "w") d = {} for c in self.chains: ndof = self.bot.get_num_dof(c) if c == "torso": param = {"slew": [1.0] * ndof, "stiffness": [0.8] * ndof, "velocity": [25.0, 15.0]} else: param = {"slew": [1.0] * ndof, "stiffness": [0.4] * ndof, "velocity": [25.0] * ndof} d[c] = {"postures": vias[c], "param": param} # safe defaults f.write(yaml.safe_dump(d)) vias = {} for c in self.chains: vias[c] = [] if k == "q": self.done = True
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 get_available_images(self): all_img= [glob.glob(p + "mouth/*.tiff") for p in m3t.get_m3_animation_path()] return [item for sublist in all_img for item in sublist]
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)
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)
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: ndof = bot.get_num_dof(c) slew[c] = [] qdes[c] = [] for i in range(ndof): slew[c].append(m3t.M3Slew()) qdes[c] = nu.array(bot.get_theta_deg(c), nu.Float)
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)
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: ndof = bot.get_num_dof(c) slew[c] = [] qdes[c] = [] for i in range(ndof): slew[c].append(m3t.M3Slew()) qdes[c] = nu.array(bot.get_theta_deg(c), nu.Float)