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 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 animator(): k=postures.keys() print 'Run time [30.0]' rt=m3t.get_float(30.0) te=time.time()+rt while time.time()<te: bot.set_theta_deg('head',[0.0]*bot.get_num_dof('head')) bliker() random_posture() ear_lighting() proxy.step() time.sleep(0.1)
def test_torque_banger(self, auto=False): d = 30.0 if not auto: print '------------- Test Conditions --------------' print '1. Actuator on test-plate' print '2. Rocker arm installed' print '3. PID tuning complete for actuator_ec' print '4. Rocker arm unloaded' print 'Continue? [y]' if not m3t.get_yes_no('y'): return print 'Enter run duration (s) [30.0]' d = m3t.get_float(30.0) print 'Enable power. Hit enter to continue' raw_input() self.step() amplitude = self.test_default[self.jid]['test_banger_amp'] adc_zero = float( self.get_sensor_list_avg(['adc_torque'], 1.0, auto=True, verbose=False)['adc_torque']) print 'Zero of ', adc_zero self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE self.comp_ec.command.t_desire = int(adc_zero) self.step(1.0) nc = 2 period = 5.0 start = time.time() dt = 0 self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE while dt < d: dt = time.time() - start a = amplitude[self.jid] * math.sin(2 * math.pi * dt / period) self.comp_ec.command.t_desire = int(adc_zero + a) self.step(time_sleep=0.1) print '----------' print 'DT', dt print 'Amplitude', a print 'Current (mA)', self.comp_rt.get_current_mA() self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_OFF self.step()
def test_torque_banger(self,auto=False): d=30.0 if not auto: print '------------- Test Conditions --------------' print '1. Actuator on test-plate' print '2. Rocker arm installed' print '3. PID tuning complete for actuator_ec' print '4. Rocker arm unloaded' print 'Continue? [y]' if not m3t.get_yes_no('y'): return print 'Enter run duration (s) [30.0]' d=m3t.get_float(30.0) print 'Enable power. Hit enter to continue' raw_input() self.step() amplitude=self.test_default[self.jid]['test_banger_amp'] adc_zero=float(self.get_sensor_list_avg(['adc_torque'],1.0,auto=True,verbose=False)['adc_torque']) print 'Zero of ',adc_zero self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE self.comp_ec.command.t_desire=int(adc_zero) self.step(1.0) nc=2 period=5.0 start=time.time() dt=0 self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE while dt<d: dt=time.time()-start a=amplitude[self.jid]*math.sin(2*math.pi*dt/period) self.comp_ec.command.t_desire=int(adc_zero+a) self.step(time_sleep=0.1) print '----------' print 'DT',dt print 'Amplitude',a print 'Current (mA)',self.comp_rt.get_current_mA() self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_OFF self.step()
omnibase_shm_names = proxy.get_available_components('m3omnibase_shm') if len(omnibase_shm_names) > 0: proxy.make_safe_operational(omnibase_shm_names[0]) humanoid_shm_names = proxy.get_available_components('m3humanoid_shm') if len(humanoid_shm_names) > 0: proxy.make_safe_operational(humanoid_shm_names[0]) #Calibrate ZLift if zlift is not None: if zlift.calibrate(proxy): #Position ZLift zlift.set_mode_theta_gc() print 'Set resting Z-Lift position (0-700) [300]' des = max(0, min(700, m3t.get_float(300))) zlift.set_slew_rate_proportion(1.0) zlift.set_stiffness(1.0) zlift.set_pos_mm(des) proxy.step() print 'Setting position to', des, '. Hit return when done' raw_input() zlift.set_mode_off() proxy.step() #Calibrate Base if omni is not None: time.sleep(0.5) proxy.step() omni.calibrate(proxy) time.sleep(0.5)
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 = []
omnibase_shm_names=proxy.get_available_components('m3omnibase_shm') if len(omnibase_shm_names) > 0: proxy.make_safe_operational(omnibase_shm_names[0]) humanoid_shm_names=proxy.get_available_components('m3humanoid_shm') if len(humanoid_shm_names) > 0: proxy.make_safe_operational(humanoid_shm_names[0]) #Calibrate ZLift if zlift is not None: if zlift.calibrate(proxy): #Position ZLift zlift.set_mode_theta_gc() print 'Set resting Z-Lift position (0-700) [300]' des=max(0,min(700,m3t.get_float(300))) zlift.set_slew_rate_proportion(1.0) zlift.set_stiffness(1.0) zlift.set_pos_mm(des) proxy.step() print 'Setting position to',des,'. Hit return when done' raw_input() zlift.set_mode_off() proxy.step() #Calibrate Base if omni is not None: time.sleep(0.5) proxy.step() omni.calibrate(proxy) time.sleep(0.5)
print "-------- Mode ---------" if use_theta_gc: print "Currently in mode: THETA_GC" else: print "Currently in mode: THETA" print "------ Scaling --------" print "Stiffness: ", stiffness print "Vel avg: ", scale_vel_avg print "--------- Vias --------" print vias if k == "v": print "Select chain" c = m3t.user_select_components_interactive(chains, single=True)[0] print "Current scale for", c, ": ", scale_vel_avg[c] print "New scale: (0-2.0)" scale_vel_avg[c] = max(0, min(2.0, m3t.get_float())) if k == "s": print "Current stiffness" print "--------------------" for c in chains: print c, " : ", stiffness[c] print print "Select chain" c = m3t.user_select_components_interactive(chains, single=True)[0] print "Enter stiffness: " s = max(0, min(1.0, m3t.get_float())) stiffness[c] = [s] * len(stiffness[c]) print "New stiffness: ", c, " : ", stiffness[c] if k == "e": use_chain = {}
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=[]
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=False) for c in chains: ndof=bot.get_num_dof(c) bot.set_mode_torque(c) print 'Enter duration (s) [30.0]' d=m3t.get_float(30.0) ts=time.time() amp=nu.array([5000.0,5000.0,2500.0,2500.0,0.0,0.0,0.0]) period=nu.array([3,3.1,2.8,3.2,3.0,3.0,3.0]) nc=0 print 'Power up system. Hit enter to start' raw_input() try: while time.time()-ts<d: nc=nc+1 t=time.time()-ts if math.fmod(nc,10)==0: print '---------------------------------------------' print 'Time: ',t,'/',d proxy.step() for c in chains:
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, ']' stiffness = max(0, min(1.0, m3t.get_float(stiffness))) if k == 'd': print 'Enter step delta (m) [', step_delta, ']' step_delta = max(0, min(.25, m3t.get_float(step_delta))) if k == 'e': run_ik(proxy, bot, step_delta, arm_name, pub, viz) proxy.stop() if rviz == True: viz.stop() # ######################################################
# ########################### fields=[['adc_ext_temp'],['adc_amp_temp'],['adc_current_a','adc_current_b']] print 'Select sensor to zero' for i in range(len(fields)): print i,' : ',fields[i] sidx=m3t.get_int() sensor=fields[sidx] print 'This assumes ISS version config files' print 'All boards should be at room temp' print 'All motor powers should be off' print 'Continue [y]?' if not m3t.get_yes_no('y'): exit() if sensor[0]=='adc_ext_temp' or sensor[0]=='adc_amp_temp': print 'Enter room temp (C)' ambient=m3t.get_float() # ########################### proxy = m3p.M3RtProxy() proxy.start() cnames=proxy.get_available_components() comp_ec=[] comp_rt=[] cnames=[q for q in cnames if q.find('actuator_ec')!=-1] for c in cnames: comp_ec.append(mcf.create_component(c)) comp_rt.append(mcf.create_component(c.replace('_ec',''))) if len(comp_ec)==0: print 'No actuator_ec components found' exit()
print '-------- Mode ---------' if use_theta_gc: print 'Currently in mode: THETA_GC' else: print 'Currently in mode: THETA' print '------ Scaling --------' print 'Stiffness: ', stiffness print 'Vel avg: ', scale_vel_avg print '--------- Vias --------' print vias if k == 'v': print 'Select chain' c = m3t.user_select_components_interactive(chains, single=True)[0] print 'Current scale for', c, ': ', scale_vel_avg[c] print 'New scale: (0-2.0)' scale_vel_avg[c] = max(0, min(2.0, m3t.get_float())) if k == 's': print 'Current stiffness' print '--------------------' for c in chains: print c, ' : ', stiffness[c] print print 'Select chain' c = m3t.user_select_components_interactive(chains, single=True)[0] print 'Enter stiffness: ' s = max(0, min(1.0, m3t.get_float())) stiffness[c] = [s] * len(stiffness[c]) print 'New stiffness: ', c, ' : ', stiffness[c] if k == 'e': use_chain = {}
def experiment_a(self): print 'Prepare experiment. Hit enter when ready' raw_input() print 'Enable power. Hit enter when ready' raw_input() self.step() #send current commands out to dsp and read sensors adc_zero=self.comp_ec.status.adc_torque print 'Enter amplitude (ticks)[100]' amp=m3t.get_float(100) ns=100.0 #Sawtooth torque profile fwd=nu.arange(0,1.0,1/ns)*amp rev=nu.arange(0,-1.0,-1/ns)*amp zero=nu.zeros(int(ns)) fwd=fwd.tolist()+zero.tolist() rev=rev.tolist()+zero.tolist() #Place actuator in torque mode self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE log_adc_torque=[] log_load_mNm=[] self.comp_ec.command.t_desire=0 #Initial desired torque #Send out intial configuration (zero torque) and wait 1 second self.step(time_sleep=1.0) for f in fwd: self.comp_ec.command.t_desire=int(f+adc_zero) #send out command torque self.step(time_sleep=0.05) #send new desired out and wait lmNm=self.dpm3.get_load_mNm()#read load-cell tq=self.comp_ec.status.adc_torque #read raw torque sensor (ticks) print '------------------------' print 'Fwd',f print 'Adc SEA',tq print 'mNm Load Cell',lmNm log_adc_torque.append(tq) #log data log_load_mNm.append(lmNm) for r in rev: self.comp_ec.command.t_desire=int(r+adc_zero) #send out command torque self.step(time_sleep=0.05) lmNm=self.dpm3.get_load_mNm()#-load_zero tq=self.comp_ec.status.adc_torque print '------------------------' print 'Rev',r print 'Adc SEA',tq print 'mNm Load Cell',lmNm log_adc_torque.append(tq) log_load_mNm.append(lmNm) self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_OFF #Turn off actuator self.step()#send out command #Least squares fit to get nominal calibration poly,inv_poly=self.get_polyfit_to_data(x=log_adc_torque,y=log_load_mNm,n=1,draw=False) #Compute calibrated view of raw torque data s=m3tc.PolyEval(poly,log_adc_torque) #Draw the comparison between loadcell and torque sensor m3t.mplot2(range(len(log_adc_torque)),log_load_mNm,s,xlabel='Samples',ylabel='Torque (mNm)', y1name='loadcell',y2name='actuator') #Save raw data self.write_raw_calibration({'log_adc_torque':log_adc_torque,\ 'log_load_mNm':log_load_mNm})
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 '---------------------------------------------' 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)
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=False) for c in chains: ndof = bot.get_num_dof(c) bot.set_mode_torque(c) print 'Enter duration (s) [30.0]' d = m3t.get_float(30.0) ts = time.time() amp = nu.array([5000.0, 5000.0, 2500.0, 2500.0, 0.0, 0.0, 0.0]) period = nu.array([3, 3.1, 2.8, 3.2, 3.0, 3.0, 3.0]) nc = 0 print 'Power up system. Hit enter to start' raw_input() try: while time.time() - ts < d: nc = nc + 1 t = time.time() - ts if math.fmod(nc, 10) == 0: print '---------------------------------------------' print 'Time: ', t, '/', d proxy.step() for c in chains:
# ########################### fields = [["adc_ext_temp"], ["adc_amp_temp"], ["adc_current_a", "adc_current_b"]] print "Select sensor to zero" for i in range(len(fields)): print i, " : ", fields[i] sidx = m3t.get_int() sensor = fields[sidx] print "This assumes ISS version config files" print "All boards should be at room temp" print "All motor powers should be off" print "Continue [y]?" if not m3t.get_yes_no("y"): exit() if sensor[0] == "adc_ext_temp" or sensor[0] == "adc_amp_temp": print "Enter room temp (C)" ambient = m3t.get_float() # ########################### proxy = m3p.M3RtProxy() proxy.start() cnames = proxy.get_available_components() comp_ec = [] comp_rt = [] cnames = [q for q in cnames if q.find("actuator_ec") != -1] for c in cnames: comp_ec.append(mcf.create_component(c)) comp_rt.append(mcf.create_component(c.replace("_ec", ""))) if len(comp_ec) == 0: print "No actuator_ec components found" exit()
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,']' stiffness=max(0,min(1.0,m3t.get_float(stiffness))) if k=='d': print 'Enter step delta (m) [',step_delta,']' step_delta=max(0,min(.25,m3t.get_float(step_delta))) if k=='e': run_ik(proxy,bot, step_delta,arm_name,pub,viz) proxy.stop() if rviz==True: viz.stop() # ######################################################
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 '---------------------------------------------' 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)