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): 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): 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 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 start(self): self.proxy.start() self.omni = m3o.MekaOmnibaseControl('meka_omnibase_control_mb2', 'meka_omnibase_control') self.proxy.subscribe_status(self.omni) self.proxy.publish_param(self.omni) self.proxy.publish_command(self.omni) joint_names=self.proxy.get_joint_components() if len(joint_names)==0: print 'No joint components available' self.proxy.stop() exit() joint_names= ['m3joint_mb2_j0', 'm3joint_mb2_j1', 'm3joint_mb2_j2', 'm3joint_mb2_j3', 'm3joint_mb2_j4', 'm3joint_mb2_j5', 'm3joint_mb2_j6', 'm3joint_mb2_j7'] 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_names = self.proxy.get_available_components('m3pwr') self.pwr = [0]*len(pwr_names) for i in range(0,len(pwr_names)): print i, pwr_names[i] pwr_rt = pwr_names[i] self.pwr[i]=m3f.create_component(pwr_rt) if self.pwr[i] is not None: self.proxy.publish_command(self.pwr[i]) self.pwr[i].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' ## 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 # Velocity limits (in rad/s) vmin = -6.0 vmax = 6.0 try: self.mode = [0] self.betad_ = [0.0]*4 self.phid_ = [0.0]*4 self.xd_ = [0.0] self.yd_ = [0.0] self.td_ = [0.0] self.tqr_ = [0.0]*4 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.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'),[0],[['Off','CC', 'Normal'],1],m3g.M3GuiWrite) self.gui.add('M3GuiSliders','Beta dot', (self,'betad_'),range(0,4),[vmin,vmax]*4,m3g.M3GuiWrite) self.gui.add('M3GuiSliders','Phi dot', (self,'phid_'),range(0,4),[vmin,vmax]*4,m3g.M3GuiWrite) self.gui.add('M3GuiSliders','Caster Ratio', (self,'tqr_'),range(0,4),[0.0,1.0]*4,m3g.M3GuiWrite) self.gui.add('M3GuiSliders','xdot', (self,'xd_'),[0],[-0.3,0.3],m3g.M3GuiWrite) self.gui.add('M3GuiSliders','ydot', (self,'yd_'),[0],[-0.3,0.3],m3g.M3GuiWrite) self.gui.add('M3GuiSliders','tdot', (self,'td_'),[0],[-1.0,1.0],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.start(self.step) except Exception,e: print "ERROR: Could not properly create GUI:", e return