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 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,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 initialize_joints(self, right_arm_settings, left_arm_settings): self.proxy = m3p.M3RtProxy() self.proxy.start(True, True) # the second true enables ROS (needed for the skin patch) for c in ['m3pwr_pwr003','m3loadx6_ma1_l0','m3arm_ma1','m3loadx6_ma2_l0','m3arm_ma2']: if not self.proxy.is_component_available(c): raise m3t.M3Exception('Component '+c+' is not available.') self.joint_list_dict = {} right_l = [] for c in ['m3joint_ma1_j0','m3joint_ma1_j1','m3joint_ma1_j2', 'm3joint_ma1_j3','m3joint_ma1_j4','m3joint_ma1_j5', 'm3joint_ma1_j6']: if not self.proxy.is_component_available(c): raise m3t.M3Exception('Component '+c+' is not available.') right_l.append(m3f.create_component(c)) self.joint_list_dict['right_arm'] = right_l left_l = [] for c in ['m3joint_ma2_j0','m3joint_ma2_j1','m3joint_ma2_j2', 'm3joint_ma2_j3','m3joint_ma2_j4','m3joint_ma2_j5', 'm3joint_ma2_j6']: if not self.proxy.is_component_available(c): raise m3t.M3Exception('Component '+c+' is not available.') left_l.append(m3f.create_component(c)) self.joint_list_dict['left_arm'] = left_l for arm,arm_settings in zip(['right_arm','left_arm'],[right_arm_settings,left_arm_settings]): if arm_settings == None: continue for comp in self.joint_list_dict[arm]: self.proxy.subscribe_status(comp) self.proxy.publish_command(comp) self.set_arm_settings(right_arm_settings,left_arm_settings) right_fts=m3.loadx6.M3LoadX6('m3loadx6_ma1_l0') self.proxy.subscribe_status(right_fts) left_fts=m3.loadx6.M3LoadX6('m3loadx6_ma2_l0') self.proxy.subscribe_status(left_fts) self.fts = {'right_arm':right_fts,'left_arm':left_fts} #self.pwr=m3w.M3Pwr('m3pwr_pwr003') self.pwr=m3f.create_component('m3pwr_pwr003') self.proxy.subscribe_status(self.pwr) self.proxy.publish_command(self.pwr) self.arms = {} self.arms['right_arm']=m3.arm.M3Arm('m3arm_ma1') self.proxy.subscribe_status(self.arms['right_arm']) self.arms['left_arm']=m3.arm.M3Arm('m3arm_ma2') self.proxy.subscribe_status(self.arms['left_arm']) self.proxy.step() self.proxy.step()
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 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 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 __init__(self): # M3-specific stuff self.omni = m3o.MekaOmnibaseControl('meka_omnibase_control_mb2', 'meka_omnibase_control') self.proxy = m3p.M3RtProxy() self.proxy.start() self.proxy.make_operational_all() self.proxy.subscribe_status(self.omni) self.proxy.publish_command(self.omni) # TODO: Use the method associated with omnibase instead. pwr_name = self.proxy.get_available_components('m3pwr') self.pwr = m3f.create_component(pwr_name[1]) self.proxy.subscribe_status(self.pwr) self.proxy.publish_command(self.pwr) self.pwr.set_motor_power_on() self.proxy.step() # ROS stuff self.sub_cmd_vel = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_cb, queue_size=1) self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=1) self.tf_bc = tf.TransformBroadcaster() self.max_lin = rospy.get_param("~max_lin_vel", 0.30) self.max_ang = rospy.get_param("~max_ang_vel", 1.00) self.cmd_vel = Twist() self.last_cmd = rospy.Time(0) self.timeout = rospy.Duration(0.25) self.diag_i = 0
def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, simple_traj_server.msg.TrajAction, execute_cb=self.execute_cb) self.proxy = m3p.M3RtProxy() self.proxy.start() bot_name=m3t.get_robot_name() self.bot=m3f.create_component(bot_name) self.chains = bot.get_available_chains() self.bot.initialize(proxy)
def __init__(self, proxy, bot, r_hand_ua_num=None, stride_ms=100): self.p = subprocess.Popen( ['roslaunch', 'm3_defs_ros', 'm3_launch.launch']) rospy.init_node("joint_state_publisher") pub = rospy.Publisher("/joint_states", JointState) time.sleep(4.0) self.bot = bot self.proxy = proxy self.pub = pub self.sleep_time = stride_ms / 1000.0 self.joints = [] self.positions = [] self.chain_names = self.bot.get_available_chains() self.use_sim = False self.r_hand_ua_present = False self.ndof_finger = 3 self.flex_factor_index = [0.3] * self.ndof_finger self.flex_factor_ring = [0.3] * self.ndof_finger self.flex_factor_pinky = [0.3] * self.ndof_finger self.flex_factor_thumb = [0.3] * 2 for chain in self.chain_names: self.positions += list(self.bot.get_theta_rad(chain)) self.joints += self.bot.get_joint_names(chain) self.ndof_hand_ua = 12 hand_names = self.proxy.get_available_components('m3hand') self.hands = [] self.hand_nums = [] for i in range(len(hand_names)): self.hands.append(m3f.create_component(hand_names[i])) self.proxy.subscribe_status(self.hands[i]) #self.proxy.publish_command(self.hands[i]) if hand_names[i][-2].isdigit(): self.hand_nums.append(hand_names[i][-2:]) else: self.hand_nums.append(hand_names[i][-1]) #r_hand_ua_num = 14 self.ndof_hand_ua = 12 for j in range(len(self.hands)): for i in range(self.ndof_hand_ua): self.positions.append(0.0) self.joints.append('m3joint_ua_mh' + str(self.hand_nums[j]) + '_j' + str(i)) # Thumb: J0,J1,J2 # Index: J3, J4, J5 # Ring: J6,J7,J8 # Pinkie: J9, J10, J11 '''if not r_hand_ua_num is None:
def __init__ (self,proxy,bot,r_hand_ua_num=None,stride_ms=100): self.p = subprocess.Popen(['roslaunch', 'm3_defs_ros', 'm3_launch.launch']) rospy.init_node("joint_state_publisher") pub = rospy.Publisher("/joint_states", JointState) time.sleep(4.0) self.bot=bot self.proxy=proxy self.pub = pub self.sleep_time = stride_ms/1000.0 self.joints = [] self.positions = [] self.chain_names = self.bot.get_available_chains() self.use_sim = False self.r_hand_ua_present = False self.ndof_finger = 3 self.flex_factor_index = [0.3] * self.ndof_finger self.flex_factor_ring = [0.3] * self.ndof_finger self.flex_factor_pinky = [0.3] * self.ndof_finger self.flex_factor_thumb = [0.3] * 2 for chain in self.chain_names: self.positions += list(self.bot.get_theta_rad(chain)) self.joints += self.bot.get_joint_names(chain) self.ndof_hand_ua = 12 hand_names=self.proxy.get_available_components('m3hand') self.hands = [] self.hand_nums = [] for i in range(len(hand_names)): self.hands.append(m3f.create_component(hand_names[i])) self.proxy.subscribe_status(self.hands[i]) #self.proxy.publish_command(self.hands[i]) if hand_names[i][-2].isdigit(): self.hand_nums.append(hand_names[i][-2:]) else: self.hand_nums.append(hand_names[i][-1]) #r_hand_ua_num = 14 self.ndof_hand_ua = 12 for j in range(len(self.hands)): for i in range(self.ndof_hand_ua): self.positions.append(0.0) self.joints.append('m3joint_ua_mh'+str(self.hand_nums[j])+'_j'+str(i)) # Thumb: J0,J1,J2 # Index: J3, J4, J5 # Ring: J6,J7,J8 # Pinkie: J9, J10, J11 '''if not r_hand_ua_num is None:
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 __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer( self._action_name, simple_traj_server.msg.TrajAction, execute_cb=self.execute_cb) self.proxy = m3p.M3RtProxy() self.proxy.start() bot_name = m3t.get_robot_name() self.bot = m3f.create_component(bot_name) self.chains = bot.get_available_chains() self.bot.initialize(proxy)
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 = m3f.create_component(bot_name) self.bot.initialize(self.proxy) csp_name = self.proxy.get_available_components('m3head_s2csp_ctrl') if len(csp_name) != 1: print 'CSP component not available' exit() self.csp_rt = m3csp.M3HeadS2CSPCtrl(csp_name[0]) self.proxy.subscribe_status(self.csp_rt) self.proxy.publish_command(self.csp_rt) self.proxy.publish_param(self.csp_rt) self.mode = [0] self.lat = [0] self.longi = [0] self.theta_j2 = [0] self.ts_rand = 0 self.la = 0 self.lo = 0 self.joints = range(7) 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=1) self.gui.add('M3GuiTree', 'Param', (self, 'param_dict'), [], [], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiModes', 'Mode', (self, 'mode'), range(1), [['Off', 'CSP', 'CSPRandom'], 1], m3g.M3GuiWrite) self.gui.add('M3GuiSliders', 'Left/Right (Deg)', (self, 'longi'), [0], [-90, 90], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'Up/Down (Deg)', (self, 'lat'), [0], [-90, 90], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'Head Roll (Deg)', (self, 'theta_j2'), [0], [-15, 15], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiToggle', 'Save', (self, 'save'), [], [['On', 'Off']], m3g.M3GuiWrite) self.gui.start(self.step)
def test_sensor_in_range(self, config, proxy): err_cnt = 0 err_msg = [] for k in config: #print 'Starting', k, '..' for j in range(len(config[k])): method = config[k][j]['method'] mmin = config[k][j]['min'] mmax = config[k][j]['max'] nsteps = config[k][j]['nsteps'] comp = m3f.create_component(k) proxy.subscribe_status(comp, verbose=False) proxy.publish_param(comp) proxy.step() fp = True for i in range(nsteps): error = False proxy.step() fcn = getattr(comp, method) value = fcn() if value < mmin or value > mmax: error = True msg = comp.name + ' ' + method + ' val: ' + str( value) + ' min: ' + str(mmin) + ' max: ' + str( mmax) + ' ' + str(i) res = { 'Test': 'SensorInRange', 'Error': True, 'Msg': msg } if fp: m = 'Test: SensorInRange: FAIL: \t' + comp.name + ' : \t' + method + ' : \t' + "%3.2f" % value err_msg.append(m) print err_msg[-1] err_cnt = err_cnt + 1 fp = False else: msg = comp.name + ' ' + method + ' val: ' + str( value) + ' min: ' + str(mmin) + ' max: ' + str( mmax) + ' ' + str(i) res = { 'Test': 'SensorInRange', 'Error': False, 'Msg': msg } if fp: print 'Test: SensorInRange: Pass: \t', comp.name, ' : \t', method, ' : \t', "%3.2f" % value fp = False self.log.append(res) return err_cnt, err_msg
def get_fields(self, req): """ Callback for the get_fields service request """ request_name = req.component if request_name not in self.comps.keys(): try: comp = mcf.create_component(str(request_name)) self.rt_proxy.subscribe_status(comp) except AttributeError, e: rospy.logwarn("No fields available for %s", request_name) return self.comps[request_name] = comp
def do_plot(proxy): print 'Starting plot...' print 'Enter log name [foo]' logname=m3t.get_string('foo') ns=proxy.get_log_num_samples(logname) if ns==0: return print '-------- Available components --------' names=proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [',names[0],']' name=m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ',ns print 'Enter start sample idx: [0]' start=max(0,m3t.get_int(0)) print 'Enter end sample idx: [',ns-1,']' end=min(ns-1,m3t.get_int(ns-1)) print '--------------------------------------' print ' Select field to plot...' comp=mcf.create_component(name) proxy.register_log_component(comp) field=m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status,field),'__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) print 'Fetching data...' data=[] for i in range(start,end): proxy.load_log_sample(logname,i) if repeated: v=m3t.get_msg_field_value(comp.status,field)[idx] else: v=m3t.get_msg_field_value(comp.status,field) data.append(v) # m3t.plot(data) m3t.mplot(data,range(len(data)))
def do_plot(proxy): print 'Starting plot...' print 'Enter log name [foo]' logname = m3t.get_string('foo') ns = proxy.get_log_num_samples(logname) if ns == 0: return print '-------- Available components --------' names = proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [', names[0], ']' name = m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ', ns print 'Enter start sample idx: [0]' start = max(0, m3t.get_int(0)) print 'Enter end sample idx: [', ns - 1, ']' end = min(ns - 1, m3t.get_int(ns - 1)) print '--------------------------------------' print ' Select field to plot...' comp = mcf.create_component(name) proxy.register_log_component(comp) field = m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) print 'Fetching data...' data = [] for i in range(start, end): proxy.load_log_sample(logname, i) if repeated: v = m3t.get_msg_field_value(comp.status, field)[idx] else: v = m3t.get_msg_field_value(comp.status, field) data.append(v) # m3t.plot(data) m3t.mplot(data, range(len(data)))
def start(self): self.proxy.start() cnames = self.proxy.get_available_components() print 'Available components:', cnames self.names = m3t.user_select_components_interactive(cnames) self.components = [] for name in self.names: self.components.append(m3f.create_component(name)) self.proxy.subscribe_status(self.components[-1]) self.proxy.publish_command(self.components[-1]) self.proxy.publish_param(self.components[-1]) self.proxy.make_operational_all() self.proxy.step()
def start(self): self.proxy.start() cnames=self.proxy.get_available_components() print 'Available components:', cnames self.names=m3t.user_select_components_interactive(cnames) self.components=[] for name in self.names: self.components.append(m3f.create_component(name)) self.proxy.subscribe_status(self.components[-1]) self.proxy.publish_command(self.components[-1]) self.proxy.publish_param(self.components[-1]) self.proxy.make_operational_all() self.proxy.step()
def do_scope(proxy): print 'Starting scope...' print 'Enter log name [foo]' logname=m3t.get_string('foo') ns=proxy.get_log_num_samples(logname) print 'Num samples available: ',ns if ns==0: return print '-------- Available components --------' names=proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [',names[0],']' name=m3t.get_string(names[0]) comp=mcf.create_component(name) proxy.register_log_component(comp) scope=m3t.M3Scope(xwidth=ns-1) print '--------------------------------------' print 'Num samples available: ',ns print 'Enter start sample idx: [0]' start=max(0,m3t.get_int(0)) print 'Enter end sample idx: [',ns-1,']' end=min(ns-1,m3t.get_int(ns-1)) print '--------------------------------------' print 'Select field to monitor' field=m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status,field),'__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) for i in range(start,end): proxy.load_log_sample(logname,i) if repeated: v=m3t.get_msg_field_value(comp.status,field)[idx] else: v=m3t.get_msg_field_value(comp.status,field) scope.plot(v) time.sleep(0.05)
def do_scope(proxy): print 'Starting scope...' print 'Enter log name [foo]' logname = m3t.get_string('foo') ns = proxy.get_log_num_samples(logname) print 'Num samples available: ', ns if ns == 0: return print '-------- Available components --------' names = proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [', names[0], ']' name = m3t.get_string(names[0]) comp = mcf.create_component(name) proxy.register_log_component(comp) scope = m3t.M3Scope(xwidth=ns - 1) print '--------------------------------------' print 'Num samples available: ', ns print 'Enter start sample idx: [0]' start = max(0, m3t.get_int(0)) print 'Enter end sample idx: [', ns - 1, ']' end = min(ns - 1, m3t.get_int(ns - 1)) print '--------------------------------------' print 'Select field to monitor' field = m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) for i in range(start, end): proxy.load_log_sample(logname, i) if repeated: v = m3t.get_msg_field_value(comp.status, field)[idx] else: v = m3t.get_msg_field_value(comp.status, field) scope.plot(v) time.sleep(0.05)
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()
def test_sensor_in_range(self, config, proxy): err_cnt=0 err_msg=[] for k in config: #print 'Starting', k, '..' for j in range(len(config[k])): method = config[k][j]['method'] mmin = config[k][j]['min'] mmax = config[k][j]['max'] nsteps = config[k][j]['nsteps'] comp = m3f.create_component(k) proxy.subscribe_status(comp,verbose=False) proxy.publish_param(comp) proxy.step() fp=True for i in range(nsteps): error = False proxy.step() fcn = getattr(comp,method) value = fcn() if value < mmin or value > mmax: error = True msg=comp.name + ' ' + method + ' val: ' + str(value) + ' min: ' + str(mmin) + ' max: ' + str(mmax) + ' ' + str(i) res={'Test':'SensorInRange','Error':True,'Msg':msg} if fp: m= 'Test: SensorInRange: FAIL: \t'+comp.name+' : \t'+method+' : \t'+"%3.2f" % value err_msg.append(m) print err_msg[-1] err_cnt=err_cnt+1 fp=False else: msg=comp.name + ' ' + method + ' val: ' + str(value) + ' min: ' + str(mmin) + ' max: ' + str(mmax) + ' ' + str(i) res={'Test':'SensorInRange','Error':False,'Msg':msg} if fp: print 'Test: SensorInRange: Pass: \t',comp.name,' : \t',method,' : \t',"%3.2f" % value fp=False self.log.append(res) return err_cnt,err_msg
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=m3f.create_component(bot_name) self.bot.initialize(self.proxy) csp_name=self.proxy.get_available_components('m3head_s2csp_ctrl') if len(csp_name)!=1: print 'CSP component not available' exit() self.csp_rt=m3csp.M3HeadS2CSPCtrl(csp_name[0]) self.proxy.subscribe_status(self.csp_rt) self.proxy.publish_command(self.csp_rt) self.proxy.publish_param(self.csp_rt) self.mode=[0] self.lat=[0] self.longi=[0] self.theta_j2=[0] self.ts_rand=0 self.la=0 self.lo=0 self.joints=range(7) 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=1) self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(1),[['Off','CSP','CSPRandom'],1],m3g.M3GuiWrite) self.gui.add('M3GuiSliders','Left/Right (Deg)', (self,'longi'),[0],[-90,90],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Up/Down (Deg)', (self,'lat'),[0],[-90,90],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Head Roll (Deg)', (self,'theta_j2'),[0],[-15,15],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite) self.gui.start(self.step)
def start(self): self.proxy.start() self.ndof_finger = 3 self.flex_factor_index = [0.3] * self.ndof_finger self.flex_factor_ring = [0.3] * self.ndof_finger self.flex_factor_pinky = [0.3] * self.ndof_finger self.flex_factor_thumb = [0.3] * 2 self.p = subprocess.Popen(['roslaunch', 'm3_defs_ros', 'm3_launch.launch']) rospy.init_node("joint_state_publisher") self.pub = rospy.Publisher("/joint_states", JointState) time.sleep(4.0) hand_names=self.proxy.get_available_components('m3hand') self.hands = [] self.hand_nums = [] for i in range(len(hand_names)): self.hands.append(m3f.create_component(hand_names[i])) self.proxy.subscribe_status(self.hands[i]) #self.proxy.publish_command(self.hands[i]) if hand_names[i][-2].isdigit(): self.hand_nums.append(hand_names[i][-2:]) else: self.hand_nums.append(hand_names[i][-1]) #r_hand_ua_num = 14 self.ndof_hand_ua = 12 self.positions = [] self.joints = [] for j in range(len(self.hands)): for i in range(self.ndof_hand_ua): self.positions.append(0.0) self.joints.append('m3joint_ua_mh'+str(self.hand_nums[j])+'_j'+str(i)) # Thumb: J0,J1,J2 # Index: J3, J4, J5 # Ring: J6,J7,J8 # Pinkie: J9, J10, J11 print 'Starting hand viz.' while(True): self.positions = [] self.proxy.step() for i in range(len(self.hands)): th =self.hands[i].get_theta_rad() #Thumb self.positions.append(-th[0]+1.57) #0 self.positions.append(th[1] * self.flex_factor_thumb[0]) self.positions.append(th[1] * self.flex_factor_thumb[1]) #Index self.positions.append(th[2] * self.flex_factor_index[0]) self.positions.append(th[2] * self.flex_factor_index[1]) self.positions.append(th[2] * self.flex_factor_index[2]) #Ring self.positions.append(th[3] * self.flex_factor_ring[0]) self.positions.append(th[3] * self.flex_factor_ring[1]) self.positions.append(th[3] * self.flex_factor_ring[2]) #Pinkie self.positions.append(th[4] * self.flex_factor_pinky[0]) self.positions.append(th[4] * self.flex_factor_pinky[1]) self.positions.append(th[4] * self.flex_factor_pinky[2]) if self.pub is not None and not rospy.is_shutdown(): header = Header(0,rospy.Time.now(),'0') self.pub.publish(JointState(header, self.joints, self.positions, [0]*len(self.positions), [0]*len(self.positions))) else: print 'Error...exiting.' break time.sleep(0.1)
def main(): proxy = m3p.M3RtProxy() proxy.start() 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.publish_param(omni) # we need this for calibration proxy.subscribe_status(omni) proxy.publish_command(omni) pwr_name = [m3t.get_omnibase_pwr_component_name(base_name[0])] #proxy.get_available_components('m3pwr') #if len(pwr_name)>1: #pwr_name=m3t.user_select_components_interactive(pwr_name,single=True) pwr = m3f.create_component(pwr_name[0]) proxy.subscribe_status(pwr) proxy.publish_command(pwr) proxy.make_operational(pwr_name[0]) proxy.step() omni.set_mode_off() pwr.set_motor_power_on() proxy.make_operational_all() proxy.step() time.sleep(0.5) proxy.step() omni.calibrate(proxy) time.sleep(0.5) print "Turn power on to robot and press any key." raw_input() omni.set_local_position(0, 0, 0, proxy) omni.set_global_position(0, 0, 0, proxy) omni.set_max_linear_accel(max_lin_acc) omni.set_max_linear_velocity(max_lin_vel) omni.set_max_rotation_velocity(max_rot_vel) omni.set_max_rotation_accel(max_rot_acc) proxy.step() '''p = omni.get_global_position() print 'Pos:', p''' omni.set_mode_traj_goal() omni.set_traj_goal(0, 0, 0) proxy.step() time.sleep(4) print 'Bus voltage:', omni.get_bus_voltage() print 'Press any key to begin pacing.' raw_input() try: while True: omni.set_traj_goal(2.0, 0, 180) proxy.step() time.sleep(0.1) proxy.step() while not omni.is_traj_goal_reached(): proxy.step() p = omni.get_global_position() print 'Pos:', p time.sleep(0.1) omni.set_traj_goal(0, 0, 0) proxy.step() time.sleep(0.1) proxy.step() while not omni.is_traj_goal_reached(): proxy.step() p = omni.get_global_position() print 'Pos:', p time.sleep(0.1) except KeyboardInterrupt: pass omni.set_mode_off() pwr.set_motor_power_off() proxy.step() proxy.stop()
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)
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)
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=[]
def start(self): self.proxy.start() self.ndof_finger = 3 self.flex_factor_index = [0.3] * self.ndof_finger self.flex_factor_ring = [0.3] * self.ndof_finger self.flex_factor_pinky = [0.3] * self.ndof_finger self.flex_factor_thumb = [0.3] * 2 self.p = subprocess.Popen( ['roslaunch', 'm3_defs_ros', 'm3_launch.launch']) rospy.init_node("joint_state_publisher") self.pub = rospy.Publisher("/joint_states", JointState) time.sleep(4.0) hand_names = self.proxy.get_available_components('m3hand') self.hands = [] self.hand_nums = [] for i in range(len(hand_names)): self.hands.append(m3f.create_component(hand_names[i])) self.proxy.subscribe_status(self.hands[i]) #self.proxy.publish_command(self.hands[i]) if hand_names[i][-2].isdigit(): self.hand_nums.append(hand_names[i][-2:]) else: self.hand_nums.append(hand_names[i][-1]) #r_hand_ua_num = 14 self.ndof_hand_ua = 12 self.positions = [] self.joints = [] for j in range(len(self.hands)): for i in range(self.ndof_hand_ua): self.positions.append(0.0) self.joints.append('m3joint_ua_mh' + str(self.hand_nums[j]) + '_j' + str(i)) # Thumb: J0,J1,J2 # Index: J3, J4, J5 # Ring: J6,J7,J8 # Pinkie: J9, J10, J11 print 'Starting hand viz.' while (True): self.positions = [] self.proxy.step() for i in range(len(self.hands)): th = self.hands[i].get_theta_rad() #Thumb self.positions.append(-th[0] + 1.57) #0 self.positions.append(th[1] * self.flex_factor_thumb[0]) self.positions.append(th[1] * self.flex_factor_thumb[1]) #Index self.positions.append(th[2] * self.flex_factor_index[0]) self.positions.append(th[2] * self.flex_factor_index[1]) self.positions.append(th[2] * self.flex_factor_index[2]) #Ring self.positions.append(th[3] * self.flex_factor_ring[0]) self.positions.append(th[3] * self.flex_factor_ring[1]) self.positions.append(th[3] * self.flex_factor_ring[2]) #Pinkie self.positions.append(th[4] * self.flex_factor_pinky[0]) self.positions.append(th[4] * self.flex_factor_pinky[1]) self.positions.append(th[4] * self.flex_factor_pinky[2]) if self.pub is not None and not rospy.is_shutdown(): header = Header(0, rospy.Time.now(), '0') self.pub.publish( JointState(header, self.joints, self.positions, [0] * len(self.positions), [0] * len(self.positions))) else: print 'Error...exiting.' break time.sleep(0.1)
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)
print 'Omnibase not found. Proceeding...' else: omni=m3o.M3OmniBase(base_name[0]) proxy.publish_param(omni) # we need this for calibration proxy.subscribe_status(omni) proxy.publish_command(omni) if omni==None and zlift==None: exit() if zlift is not None: pwr_rt=m3t.get_joint_pwr_component_name(zlift_names[0]) else: pwr_rt=m3t.get_omnibase_pwr_component_name(base_name[0]) pwr=m3f.create_component(pwr_rt) proxy.publish_command(pwr) pwr.set_motor_power_on() proxy.make_operational_all() proxy.step() time.sleep(0.5) proxy.step() zlift_shm_names=proxy.get_available_components('m3joint_zlift_shm') if len(zlift_shm_names) > 0: proxy.make_safe_operational(zlift_shm_names[0]) omnibase_shm_names=proxy.get_available_components('m3omnibase_shm') if len(omnibase_shm_names) > 0: proxy.make_safe_operational(omnibase_shm_names[0])
import m3.toolbox as m3t import m3.component_factory as mcf import numpy.numarray as na import math proxy = m3p.M3RtProxy() proxy.start() cnames=proxy.get_available_components() print 'All actuator_ec [n]?' comps=[] if m3t.get_yes_no('n'): cnames=[q for q in cnames if q.find('actuator_ec')!=-1] all_aec=True for c in cnames: comps.append(mcf.create_component(c)) else: all_aec=False print '------- Components ------' for i in range(len(cnames)): print i,' : ',cnames[i] print '-------------------------' print 'Enter component id' cid=m3t.get_int() name=cnames[cid] comps.append(mcf.create_component(name)) if len(comps)==0: print 'No components selected' exit() for c in comps:
robot_name = 'm3humanoid_bimanual_mr1' chain_names = ['m3arm_ma1', 'm3arm_ma2'] dynamatics_nms = ['m3dynamatics_ma1', 'm3dynamatics_ma2'] proxy.make_safe_operational(robot_name) for c in chain_names: proxy.make_safe_operational(c) for d in dynamatics_nms: proxy.make_safe_operational(d) proxy = m3p.M3RtProxy() proxy.start() pwr_nm = 'm3pwr_pwr003' pwr = m3f.create_component(pwr_nm) proxy.publish_command(pwr) joint_names = [ 'm3joint_ma1_j0', 'm3joint_ma1_j1', 'm3joint_ma1_j2', 'm3joint_ma1_j3', 'm3joint_ma1_j4', 'm3joint_ma1_j5', 'm3joint_ma1_j6' ] comp_list = [] stiff_list = [0.2, 0.67, 1., 0.7, 0.75, 0.5, 0.5] for i, c in enumerate(joint_names): comp = m3f.create_component(c) comp_list.append(comp) proxy.publish_command(comp) if i < 5: comp.set_control_mode(THETA_GC)
def do_dump(proxy): print 'Starting dump...' print 'Enter log name [foo]' logname = m3t.get_string('foo') ns = proxy.get_log_num_samples(logname) if ns == 0: return print '-------- Available components --------' names = proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [', names[0], ']' name = m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ', ns print 'Enter start sample idx: [0]' start = max(0, m3t.get_int(0)) print 'Enter end sample idx: [', ns - 1, ']' end = min(ns - 1, m3t.get_int(ns - 1)) print '--------------------------------------' comp = mcf.create_component(name) proxy.register_log_component(comp) fields = [] print 'Dump all data?[n]' if m3t.get_yes_no('n'): '''print 'Fetching data...' fn=m3t.get_m3_log_path()+logname+'/'+logname+'_dump.yaml' f=file(fn,'w') print 'Saving...',fn f.write(yaml.safe_dump(comp.status, default_flow_style=False,width=200)) f.close()''' fields = comp.status.DESCRIPTOR.fields_by_name.keys() print fields else: print ' Select fields to plot...' while True: fields.append(m3t.user_select_msg_field(comp.status)) print 'Another field [n]?' if not m3t.get_yes_no('n'): break print 'Fetching data...' data = {} for k in fields: data[k] = [] print '---------------------------' print k print m3t.get_msg_field_value(comp.status, k) print dir(m3t.get_msg_field_value(comp.status, k)) if hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'): for j in range(len(m3t.get_msg_field_value(comp.status, k))): data[k].append([]) for i in range(start, end): proxy.load_log_sample(logname, i) for k in fields: repeated = False # skip protobuf subclasses (like base) for now. note we'll want this for humanoid if hasattr(m3t.get_msg_field_value(comp.status, k), '__metaclass__'): pass elif hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'): for j in range(len(m3t.get_msg_field_value(comp.status, k))): data[k][j].append( m3t.get_msg_field_value(comp.status, k)[j]) else: data[k].append(m3t.get_msg_field_value(comp.status, k)) fn = m3t.get_m3_log_path() + logname + '/' + logname + '_dump.yaml' f = file(fn, 'w') print 'Saving...', fn print data f.write(yaml.safe_dump(data, default_flow_style=False, width=200)) f.close()
import m3.rt_proxy as m3p import meka_omnibase_control as m3o import m3.component_factory as m3f import math import numpy import time WHEEL_COUNT = 4 if __name__=='__main__': proxy = m3p.M3RtProxy() proxy.start() pwr = m3f.create_component("m3pwr_pwr029") omni = m3o.MekaOmnibaseControl('meka_omnibase_control_mb2', 'meka_omnibase_control') proxy.make_operational_all() proxy.subscribe_status(omni) proxy.publish_command(omni) proxy.subscribe_status(pwr) proxy.publish_command(pwr) #pwr.set_motor_power_on() proxy.step() print "Omnibase calibration - Please slowly move the mobile base so that", \
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()
print 'Omnibase not found. Proceeding...' else: omni = m3o.M3OmniBase(base_name[0]) proxy.publish_param(omni) # we need this for calibration proxy.subscribe_status(omni) proxy.publish_command(omni) if omni == None and zlift == None: exit() if zlift is not None: pwr_rt = m3t.get_joint_pwr_component_name(zlift_names[0]) else: pwr_rt = m3t.get_omnibase_pwr_component_name(base_name[0]) pwr = m3f.create_component(pwr_rt) proxy.publish_command(pwr) pwr.set_motor_power_on() proxy.make_operational_all() proxy.step() time.sleep(0.5) proxy.step() zlift_shm_names = proxy.get_available_components('m3joint_zlift_shm') if len(zlift_shm_names) > 0: proxy.make_safe_operational(zlift_shm_names[0]) omnibase_shm_names = proxy.get_available_components('m3omnibase_shm') if len(omnibase_shm_names) > 0: proxy.make_safe_operational(omnibase_shm_names[0])
def main(): proxy = m3p.M3RtProxy() proxy.start() base_name=proxy.get_available_components('m3omnibase') omni=None if len(base_name)!=1: print 'Omnibase not available. Proceeding without it...' else: print 'Use OmniBase [y]?' if m3t.get_yes_no('y'): omni=m3o.M3OmniBase(base_name[0]) proxy.publish_param(omni) # we need this for calibration proxy.subscribe_status(omni) proxy.publish_command(omni) pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])] #proxy.get_available_components('m3pwr') #if len(pwr_name)>1: #pwr_name=m3t.user_select_components_interactive(pwr_name,single=True) pwr=m3f.create_component(pwr_name[0]) proxy.subscribe_status(pwr) proxy.publish_command(pwr) zlift_names=proxy.get_available_components('m3joint_zlift') zl=None if len(zlift_names)==1: print 'Use Zlift [y]?' if m3t.get_yes_no('y'): zl=m3z.M3JointZLift(zlift_names[0]) proxy.subscribe_status(zl) proxy.publish_command(zl) proxy.publish_param(zl) proxy.make_operational(pwr_name[0]) proxy.step() if omni is not None: omni.set_mode_off() pwr.set_motor_power_on() proxy.make_operational_all() zlift_shm_names=proxy.get_available_components('m3joint_zlift_shm') if len(zlift_shm_names) > 0: proxy.make_safe_operational(zlift_shm_names[0]) 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]) m3ledx2xn_ec_shm_names=proxy.get_available_components('m3ledx2xn_ec_shm') if len(m3ledx2xn_ec_shm_names) > 0: proxy.make_safe_operational(m3ledx2xn_ec_shm_names[0]) m3led_matrix_ec_shm_names=proxy.get_available_components('m3led_matrix_ec_shm') if len(m3led_matrix_ec_shm_names) > 0: proxy.make_safe_operational(m3led_matrix_ec_shm_names[0]) proxy.step() time.sleep(0.5) if omni is not None: proxy.step() omni.calibrate(proxy) time.sleep(0.5) if zl is not None: if not zl.calibrate(proxy): zl=None print 'ZLift failed to calibrate' if omni is None and zl is None: exit() print "Turn motor power on to Omnibase and press any key." raw_input() joy=m3to.M3OmniBaseJoy() joy.start(proxy,omni,zl) k = 0 try: while True: joy.step() #print 'Bus Current:', pwr.get_bus_torque() p = omni.get_local_position() #omni.set_op_space_forces(f.jx*200.0, f.jy*200.0, f.jyaw*50.0) k += 1 if k == 100: print '-----------Local Pos-------' print 'X:', p[0] print 'Y:', p[1] print 'Yaw:', p[2] #print '---------------------------' k = 0 '''print '-------Joystick Pos-------' print 'jx:',f.jx print 'jy:', f.jy print 'jyaw:', f.jyaw print 'button:', f.jbutton print '---------------------------' #print 'Bus voltage',omni.get_bus_voltage()''' '''tqs = omni.get_motor_torques() print tqs[0], tqs[2], tqs[4], tqs[6]''' #print omni.get_steer_torques() proxy.step() except KeyboardInterrupt: pass joy.stop() if omni is not None: omni.set_mode_off() pwr.set_motor_power_off() proxy.step() proxy.stop()
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 main(): proxy = m3p.M3RtProxy() proxy.start() 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.publish_param(omni) # we need this for calibration proxy.subscribe_status(omni) proxy.publish_command(omni) pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])] #proxy.get_available_components('m3pwr') #if len(pwr_name)>1: #pwr_name=m3t.user_select_components_interactive(pwr_name,single=True) pwr=m3f.create_component(pwr_name[0]) proxy.subscribe_status(pwr) proxy.publish_command(pwr) proxy.make_operational(pwr_name[0]) proxy.step() pwr.set_motor_power_on() proxy.step() proxy.make_operational_all() proxy.step() time.sleep(0.5) proxy.step() omni.calibrate(proxy) time.sleep(0.5) omni.set_local_position(0,0,0,proxy) omni.set_global_position(0,0,0,proxy) omni.set_mode_traj_via() '''omni.add_via(0, 1.2, -180) omni.add_via(1, 1.2, 0) omni.add_via(1, 0, 180) omni.add_via(0, 0, 0)''' omni.add_via(0, 0.2, 0) omni.add_via(0.2, 0.2, 0) omni.add_via(0.2, 0, 0) omni.add_via(0, 0, 0) proxy.step() time.sleep(0.2) proxy.step() '''while not omni.is_traj_goal_reached(): proxy.step()''' try: while True: proxy.step() p = omni.get_global_position() d = omni.get_desired_position() print '-----------Local Pos-------' print 'X:', p[0] print 'Y:', p[1] print 'Yaw:', p[2] print '---------------------------' print '-----------Desired Pos-------' print 'dX:', d[0] print 'dY:', d[1] print 'dYaw:', d[2] print '---------------------------' time.sleep(0.1) except (KeyboardInterrupt): pass stop()
print '------- Components ------' for i in range(len(comps)): print i, ' : ', comps[i] print '-------------------------' print 'Enter component id' cid = m3t.get_int() print 'Select Y-Range? [n]' yrange = None if m3t.get_yes_no('n'): yrange = [] print 'Min?' yrange.append(m3t.get_int()) print 'Max?' yrange.append(m3t.get_int()) name = comps[cid] comp = mcf.create_component(name) proxy.subscribe_status(comp) #proxy.publish_param(comp) field = m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) scope = m3t.M3Scope(xwidth=100, yrange=yrange) try: ts = time.time() while True: proxy.step()
import pylab as pyl import time import m3.rt_proxy as m3p import m3.toolbox as m3t import m3.component_factory as mcf import numpy.numarray as na import math proxy = m3p.M3RtProxy() proxy.start() cnames = proxy.get_available_components() cnames = [q for q in cnames if q.find('actuator_ec') != -1] cnames = [q for q in cnames if q.find('mh') == -1] comps = [] for c in cnames: comps.append(mcf.create_component(c)) proxy.subscribe_status(comps[-1]) last_period = [] last_rollover = [] offset_period = [] offset_rollover = [] for i in range(len(comps)): last_period.append(0) last_rollover.append(0) offset_period.append(0) offset_rollover.append(0) try: ts = time.time() terr_q = 0 terr_tq = 0 while True:
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' print 'q: quit' print '--------------' print k=m3t.get_keystroke() if k=='q': break
print 'No components found' exit() print 'Using components: ' for i in range(len(cnames)): print i, ':', cnames[i] print 'Continue [y]?' if not m3t.get_yes_no('y'): exit() print 'Query to save each calibration [y]?' do_query = m3t.get_yes_no('y') for c in comp.keys(): comp[c]['comp_rt'] = mcf.create_component(c.replace('_ec', '')) comp[c]['comp_j'] = mcf.create_component(c.replace('actuator_ec', 'joint')) proxy.subscribe_status(comp[c]['comp_rt']) proxy.subscribe_status(comp[c]['comp_j']) #proxy.publish_param(comp[c]['comp_j']) proxy.make_operational_all() proxy.step() # ########################### ns = 30 for i in range(ns): proxy.step() print '---------' for c in comp.keys(): tqj = comp[c]['comp_j'].get_torque_mNm() if lt == 3 or lt == 4:
#POSSIBILITY OF SUCH DAMAGE. import time import m3.rt_proxy as m3p import m3.toolbox as m3t import m3.component_factory as m3f import Numeric as nu import m3.humanoid # ###################################################### 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:
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()
print 'Use RVIZ? (y/n)' print '--------------------------' print k=m3t.get_keystroke() rviz = False pub = None if k == 'y': rviz = True bot_name=m3t.get_robot_name() if bot_name == "": print 'Error: no botanoid components found:', bot_names proxy.stop() sys.exit() bot=m3f.create_component(bot_name) viz = None if rviz == True: viz = m3v.M3Viz(proxy, bot) proxy.publish_param(bot) #allow to set payload proxy.subscribe_status(bot) proxy.publish_command(bot) proxy.make_operational_all() humanoid_shm_names=proxy.get_available_components('m3humanoid_shm') if len(humanoid_shm_names) > 0: proxy.make_safe_operational(humanoid_shm_names[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 = []
def start(self): self.proxy.start() self.get_component('m3actuator') print "starting components" self.start_components(['ctrl','act','act_ec'],None) print "done starting components" # for k,v in self.comps.items(): # # accomplishes this: self.act=m3s.M3Actuator(self.comp_name) # setattr(self, k, v['type'](v['name']) ) # self.comps[k]['comp'] = getattr(self,k) # self.proxy.subscribe_status(getattr(self,k)) # self.proxy.publish_command(getattr(self,k)) # self.proxy.publish_param(getattr(self,k)) # self.proxy.make_operational(v['name']) # pwr_rt=m3t.get_actuator_ec_pwr_component_name(self.comps['act_ec']['name']) 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() self.proxy.step() #Create gui self.mode = [0] self.traj = [0] self.current = [0] self.theta = [0] self.torque = [0] self.torque_lc = [0] self.save = False self.save_last = False self.do_scope = False self.scope = None # extract status fields self.status_dict = self.proxy.get_status_dict() self.ctrl_scope_keys = m3t.get_msg_fields(self.ctrl.status,prefix='',exclude=['ethercat','base']) self.ctrl_scope_keys.sort() self.ctrl_scope_keys = ['None']+self.ctrl_scope_keys print self.ctrl_scope_keys self.ctrl_scope_field1 = [0] self.ctrl_scope_field2 = [0] self.ec_scope_field1 = [0] current_max = 3.5 theta_max = 100.0 torque_max = 40.0 self.param_dict=self.proxy.get_param_dict() print str(self.param_dict) self.modes = mec._CTRL_SIMPLE_MODE.values_by_number self.mode_names = [self.modes[i].name for i in sorted(self.modes.keys())] self.trajs = mec._CTRL_SIMPLE_TRAJ_MODE.values_by_number self.traj_names = [self.trajs[i].name for i in sorted(self.trajs.keys())] 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(1),[self.mode_names,1],m3g.M3GuiWrite) self.gui.add('M3GuiModes', 'Traj', (self,'traj'),range(1),[self.traj_names,1],m3g.M3GuiWrite) # self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(1),[['Off','Current','Theta','Torque','Torque_LC'],1],m3g.M3GuiWrite) # self.gui.add('M3GuiModes', 'Traj', (self,'traj'),range(1),[['Off','Current Square','Current Sine','Theta Square','Theta Sine','Torque Square','Torque Sine','TorqueLC Square','TorqueLC Sine'],1],m3g.M3GuiWrite) self.gui.add('M3GuiSliders','Current', (self,'current'),range(1), [-current_max,current_max],m3g.M3GuiWrite) self.gui.add('M3GuiSliders','Theta(deg)', (self,'theta'),range(1),[-theta_max,theta_max],m3g.M3GuiWrite) self.gui.add('M3GuiSliders','Torque', (self,'torque'),range(1),[-torque_max,torque_max],m3g.M3GuiWrite) self.gui.add('M3GuiSliders','TorqueLC', (self,'torque_lc'),range(1),[-torque_max,torque_max],m3g.M3GuiWrite) self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite) self.gui.add('M3GuiModes', 'Ctrl Scope1', (self,'ctrl_scope_field1'),range(1),[self.ctrl_scope_keys,1],m3g.M3GuiWrite) self.gui.add('M3GuiModes', 'Ctrl Scope2', (self,'ctrl_scope_field2'),range(1),[self.ctrl_scope_keys,1],m3g.M3GuiWrite) self.gui.add('M3GuiToggle', 'Scope', (self,'do_scope'),[],[['On','Off']],m3g.M3GuiWrite) 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)