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 = m3.humanoid.M3Humanoid(bot_name) arm_names = self.bot.get_available_chains() arm_names = [x for x in arm_names if x.find('arm') != -1] if len(arm_names) == 0: print 'No arms found' return if len(arm_names) == 1: self.arm_name = arm_names[0] else: self.arm_name = m3t.user_select_components_interactive( arm_names, single=True)[0] # ####### Setup Proxy ############# self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot) self.proxy.make_operational_all() self.bot.set_motor_power_on() self.ndof = self.bot.get_num_dof(self.arm_name) self.theta_curr = [0.0] * self.ndof zlift_shm_names = self.proxy.get_available_components( 'm3joint_zlift_shm') if len(zlift_shm_names) > 0: self.proxy.make_safe_operational(zlift_shm_names[0]) omnibase_shm_names = self.proxy.get_available_components( 'm3omnibase_shm') if len(omnibase_shm_names) > 0: self.proxy.make_safe_operational(omnibase_shm_names[0]) 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]) print 'WARNING: Before raising E-Stop move a slider to initialize positions.' #Create gui self.mode = [0] self.theta_desire_a = [0] * self.num_dof 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','Current','Torque','Torque_GC','Theta','Theta_IMP','Jnt_Theta','No Brake'],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.start(self.step)
def meka_fk_server(): rospy.init_node('meka_fk_node') bot_name=m3t.get_robot_name() global bot bot = m3.humanoid.M3Humanoid(bot_name) s = rospy.Service('meka_fk', MekaFK, meka_fk_srv) print "Ready to service FK request." rospy.spin()
def meka_ik_server(): rospy.init_node("meka_ik_node") bot_name = m3t.get_robot_name() global bot bot = m3.humanoid.M3Humanoid(bot_name) s = rospy.Service("meka_ik", MekaIK, meka_ik_srv) print "Ready to service IK request." rospy.spin()
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 run(self): vias = {} for c in self.chains: vias[c] = [] while not self.done: print '--------------' print 'r: record via' print 's: save via file' print 'p: print current pose' print 'q: quit' print '--------------' print k = m3t.get_keystroke() print 'Dbg', dbg if k == 'r': print '-------------' for c in self.chains: vias[c].append(m3t.float_list(self.bot.get_theta_deg(c))) print 'Record of: ', vias[c][-1], 'for', c if k == 'p': print '----------------' for c in self.chains: print 'Chain:', c, ' : ', self.bot.get_theta_deg(c) print '----------------' if k == 's': bot_name = m3t.get_robot_name() fn = bot_name + '_' + m3t.time_string() print 'Enter via file name [', fn, ']' fn = m3t.get_string(fn) fn = m3t.get_m3_animation_path() + fn + '.via' print 'Writing file: ', fn f = file(fn, 'w') d = {} for c in self.chains: ndof = self.bot.get_num_dof(c) if c == 'torso': param = { 'slew': [1.0] * ndof, 'stiffness': [0.8] * ndof, 'velocity': [25.0, 15.0] } else: param = { 'slew': [1.0] * ndof, 'stiffness': [0.4] * ndof, 'velocity': [25.0] * ndof } d[c] = { 'postures': vias[c], 'param': param } #safe defaults f.write(yaml.safe_dump(d)) vias = {} for c in self.chains: vias[c] = [] if k == 'q': self.done = True
def start(self): self.proxy.start() bot_name = m3t.get_robot_name() if bot_name == "": print 'Error: no robot components found:', bot_names return self.bot = m3h.M3Humanoid(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.x = [5] self.y = [0] self.z = [0] self.theta_j2 = [0] self.ts_rand = 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', 'x', (self, 'x'), [0], [-10, 10], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'y', (self, 'y'), [0], [-10, 10], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'z', (self, 'z'), [0], [-10, 10], 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 __init__ (self,stride_ms=100): Thread.__init__(self) self.stride_ms = stride_ms self.proxy = m3p.M3RtProxy() self.proxy.start(True,True) bot_name = m3t.get_robot_name() bot = m3.humanoid.M3Humanoid(bot_name) self.proxy.subscribe_status(bot) self.viz = m3v.M3Viz(self.proxy,bot) self.done = False
def get_bot(proxy): assert isinstance(proxy,m3p.M3RtProxy) ## Returns the bot object, None if none or more is found bot = None try: bot_name = m3t.get_robot_name() bot = m3h.M3Humanoid(bot_name) proxy.subscribe_status(bot) except Exception,e: print 'Something went wrong when trying to get the bot : ',e bot = None
def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, meka_trajectory.msg.TrajAction, execute_cb=self.execute_cb) self.proxy = m3p.M3RtProxy() self.proxy.start() bot_name = m3t.get_robot_name() self.bot = m3h.M3Humanoid(bot_name) self.chains = self.bot.get_available_chains() self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot)
def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer( self._action_name, meka_trajectory.msg.TrajAction, execute_cb=self.execute_cb ) self.proxy = m3p.M3RtProxy() self.proxy.start() bot_name = m3t.get_robot_name() self.bot = m3h.M3Humanoid(bot_name) self.chains = self.bot.get_available_chains() self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot)
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 = m3.humanoid.M3Humanoid(bot_name) arm_names = self.bot.get_available_chains() arm_names = [x for x in arm_names if x.find("arm") != -1] if len(arm_names) == 0: print "No arms found" return if len(arm_names) == 1: self.arm_name = arm_names[0] else: self.arm_name = m3t.user_select_components_interactive(arm_names, single=True)[0] # ####### Setup Proxy ############# self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot) self.proxy.make_operational_all() self.bot.set_motor_power_on() self.ndof = self.bot.get_num_dof(self.arm_name) self.theta_curr = [0.0] * self.ndof zlift_shm_names = self.proxy.get_available_components("m3joint_zlift_shm") if len(zlift_shm_names) > 0: self.proxy.make_safe_operational(zlift_shm_names[0]) omnibase_shm_names = self.proxy.get_available_components("m3omnibase_shm") if len(omnibase_shm_names) > 0: self.proxy.make_safe_operational(omnibase_shm_names[0]) 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]) print "WARNING: Before raising E-Stop move a slider to initialize positions." # Create gui self.mode = [0] self.theta_desire_a = [0] * self.num_dof 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','Current','Torque','Torque_GC','Theta','Theta_IMP','Jnt_Theta','No Brake'],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.start(self.step)
def run(self): vias = {} for c in self.chains: vias[c] = [] while not self.done: print "--------------" print "r: record via" print "s: save via file" print "p: print current pose" print "q: quit" print "--------------" print k = m3t.get_keystroke() print "Dbg", dbg if k == "r": print "-------------" for c in self.chains: vias[c].append(m3t.float_list(self.bot.get_theta_deg(c))) print "Record of: ", vias[c][-1], "for", c if k == "p": print "----------------" for c in self.chains: print "Chain:", c, " : ", self.bot.get_theta_deg(c) print "----------------" if k == "s": bot_name = m3t.get_robot_name() fn = bot_name + "_" + m3t.time_string() print "Enter via file name [", fn, "]" fn = m3t.get_string(fn) fn = m3t.get_m3_animation_path() + fn + ".via" print "Writing file: ", fn f = file(fn, "w") d = {} for c in self.chains: ndof = self.bot.get_num_dof(c) if c == "torso": param = {"slew": [1.0] * ndof, "stiffness": [0.8] * ndof, "velocity": [25.0, 15.0]} else: param = {"slew": [1.0] * ndof, "stiffness": [0.4] * ndof, "velocity": [25.0] * ndof} d[c] = {"postures": vias[c], "param": param} # safe defaults f.write(yaml.safe_dump(d)) vias = {} for c in self.chains: vias[c] = [] if k == "q": self.done = True
def start(self): self.proxy.start() bot_name=m3t.get_robot_name() if bot_name == "": print 'Error: no robot components found:', bot_names return self.bot=m3h.M3Humanoid(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.x=[5] self.y=[0] self.z=[0] self.theta_j2=[0] self.ts_rand=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','x', (self,'x'),[0],[-10,10],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','y', (self,'y'),[0],[-10,10],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','z', (self,'z'),[0],[-10,10],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() 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): # ######## 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()
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): 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 = []
else: param = {"slew": [1.0] * ndof, "stiffness": [0.4] * ndof, "velocity": [25.0] * ndof} d[c] = {"postures": vias[c], "param": param} # safe defaults f.write(yaml.safe_dump(d)) vias = {} for c in self.chains: vias[c] = [] if k == "q": self.done = True # ###################################################### proxy = m3p.M3RtProxy() proxy.start() bot_name = m3t.get_robot_name() if bot_name == "": print "Error: no robot components found:", bot_name exit() 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() proxy.step() chains = bot.get_available_chains() print "Select chains to pose" chains = m3t.user_select_components_interactive(chains) menu = menu_thread(bot, chains)
#CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #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)
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] # ####### Setup Hand ############# hand_names = self.proxy.get_available_components('m3hand') hand_name = '' if len(hand_names) > 1: hand_name = m3t.user_select_components_interactive(chain_names, single=True) if len(hand_names) == 1: hand_name = hand_names[0] if len(hand_name): self.hand = m3.hand.M3Hand(hand_name) self.proxy.publish_command(self.hand) self.proxy.subscribe_status(self.hand) else: self.hand = None # ####### Setup Proxy ############# self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot) self.proxy.make_operational_all() self.bot.set_motor_power_on() self.ndof = self.bot.get_num_dof(self.arm_name) self.via_traj = {} self.via_traj_first = True self.theta_curr = [0.0] * self.ndof # ######## Square/Circle stuff ######################### if self.arm_name == 'right_arm': self.center = [0.450, -0.28, -0.1745] else: self.center = [0.450, 0.28, -0.1745] avail_chains = self.bot.get_available_chains() for c in avail_chains: if c == 'torso': self.center[2] += 0.5079 self.jt = m3jt.JointTrajectory(7) self.axis_demo = [0, 0, 1] # ##### Generate square vias ############################ ik_vias = [] length_m = 25 / 100.0 resolution = 20 y_left = self.center[1] + length_m / 2.0 y_right = self.center[1] - length_m / 2.0 z_top = self.center[2] + length_m / 2.0 z_bottom = self.center[2] - length_m / 2.0 dy = (y_left - y_right) / nu.float(resolution) dz = (z_top - z_bottom) / nu.float(resolution) x = self.center[0] if self.arm_name == 'right_arm': # first add start point ik_vias.append([ x, y_left, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) # add top line for i in range(resolution): ik_vias.append([ x, y_left - (i + 1) * dy, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) # add right line for i in range(resolution): ik_vias.append([ x, y_right, z_top - (i + 1) * dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) # add bottom line for i in range(resolution): ik_vias.append([ x, y_right + (i + 1) * dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) # add left line for i in range(resolution): ik_vias.append([ x, y_left, z_bottom + (i + 1) * dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) else: # first add start point ik_vias.append([ x, y_right, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) # add top line for i in range(resolution): ik_vias.append([ x, y_right + (i + 1) * dy, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) # add right line for i in range(resolution): ik_vias.append([ x, y_left, z_top - (i + 1) * dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) # add bottom line for i in range(resolution): ik_vias.append([ x, y_left - (i + 1) * dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) # add left line for i in range(resolution): ik_vias.append([ x, y_right, z_bottom + (i + 1) * dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) self.via_traj['Square'] = [] # use zero position as reference for IK solver self.bot.set_theta_sim_deg(self.arm_name, [0] * self.bot.get_num_dof(self.arm_name)) for ikv in ik_vias: theta_soln = [] #print 'solving for square ik via:', ikv if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln): self.via_traj['Square'].append(theta_soln) self.bot.set_theta_sim_deg(self.arm_name, theta_soln) else: print 'WARNING: no IK solution found for via ', ikv self.bot.set_theta_sim_deg(self.arm_name, [0] * self.bot.get_num_dof(self.arm_name)) # ##### Generate circle vias ############################ ik_vias = [] diameter_m = 25.0 / 100.0 resolution = 20 x = self.center[0] for i in range(resolution * 4 + 1): dt = 2 * nu.pi / (nu.float(resolution) * 4) t = (nu.pi / 2) + i * dt if t > nu.pi: t -= 2 * nu.pi y = self.center[1] + (diameter_m / 2.0) * nu.cos(t) z = self.center[2] + (diameter_m / 2.0) * nu.sin(t) ik_vias.append([ x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2] ]) self.via_traj['Circle'] = [] # use zero position as reference for IK solver self.bot.set_theta_sim_deg(self.arm_name, [0] * self.bot.get_num_dof(self.arm_name)) for ikv in ik_vias: theta_soln = [] if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln): self.via_traj['Circle'].append(theta_soln) self.bot.set_theta_sim_deg(self.arm_name, theta_soln) else: print 'WARNING: no IK solution found for via ', ikv self.bot.set_theta_sim_deg(self.arm_name, [0] * self.bot.get_num_dof(self.arm_name)) # ##### Load Via Trajectories ############################ self.via_files = {'TrajA': 'kha1.via'} pt = m3t.get_m3_animation_path() for k in self.via_files.keys(): fn = pt + self.via_files[k] try: f = file(fn, 'r') d = yaml.safe_load(f.read()) self.via_traj[k] = d[self.arm_name] except IOError: print 'Via file', k, 'not present. Skipping...' # ##### Hand Trajectories ############################ if self.hand is not None: pf = m3t.get_m3_animation_path() + hand_name + '_postures.yml' f = file(pf, 'r') self.hand_data = yaml.safe_load(f.read()) f.close() self.hand_traj_first = True # ######## Demo and GUI ######################### self.off = False self.grasp = False self.arm_mode_names = [ 'Off', 'Zero', 'Current', 'HoldUp', 'Square', 'Circle', 'TrajA' ] self.hand_mode_names = ['Off', 'Open', 'Grasp', 'Animation'] self.arm_mode_methods = [ self.step_off, self.step_zero, self.step_current, self.step_hold_up, self.step_via_traj, self.step_via_traj, self.step_via_traj ] self.hand_mode_methods = [ self.step_hand_off, self.step_hand_open, self.step_hand_grasp, self.step_hand_animation ] self.arm_mode = [0] self.hand_mode = [0] self.poses = { 'zero': { 'right_arm': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'left_arm': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] }, 'holdup': { 'right_arm': [56.0, 26.0, -8.0, 84.0, 119.0, -36.0, 2.0], 'left_arm': [56.0, -26.0, 8.0, 84.0, -119.0, -36.0, -2.0] } } self.stiffness = [50] self.velocity = [25] self.gui.add('M3GuiModes', 'Arm Mode', (self, 'arm_mode'), range(1), [self.arm_mode_names, 1], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiToggle', 'ESTOP', (self, 'off'), [], [['Arm Enabled', 'Arm Disabled']], m3g.M3GuiWrite, column=1) if self.hand is not None: self.gui.add('M3GuiToggle', 'Grasp', (self, 'grasp'), [], [['GraspOpen', 'GraspClosed']], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiModes', 'Hand Mode', (self, 'hand_mode'), range(1), [self.hand_mode_names, 1], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'Stiffness ', (self, 'stiffness'), [0], [0, 100], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'Velocity ', (self, 'velocity'), [0], [0, 40], m3g.M3GuiWrite, column=1) self.gui.start(self.step)
def 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
#!/usr/bin/python ''' Created on Apr 28, 2014 @author: Antoine Hoarau ([email protected]) ''' import time import m3.rt_proxy as m3p import m3.humanoid as m3h import m3.toolbox as m3t if __name__ == '__main__': proxy = m3p.M3RtProxy() proxy.start() bot = m3h.M3Humanoid(m3t.get_robot_name()) bot.initialize(proxy) cnt = 0 start = time.time() while 1: try: cnt = cnt + 1 for chain in bot.get_available_chains(): bot.set_mode_theta_gc(chain) bot.set_slew_rate_proportion(chain, [1.0] * bot.get_num_dof(chain)) bot.set_stiffness(chain, [1.0] * bot.get_num_dof(chain)) bot.set_theta_deg(chain, [0.0] * bot.get_num_dof(chain)) if (time.time() - start >= 1): for chain in bot.get_available_chains(): print chain, bot.get_theta_deg(chain) print 'freq : ', cnt cnt = 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() print '--------------------------' print 'Enable RVIZ? (y/n)' print '--------------------------' print self.rviz = False if m3t.get_yes_no(): self.rviz = True sea_joint_names=self.proxy.get_joint_components() sea_joint_names=m3t.user_select_components_interactive(sea_joint_names) self.sea_joint=[] for n in sea_joint_names: self.sea_joint.append(m3f.create_component(n)) self.proxy.subscribe_status(self.sea_joint[-1]) chain_names=self.proxy.get_chain_components() self.chain=[] if len(chain_names)>0: print 'Select kinematic chain' chain_names=m3t.user_select_components_interactive(chain_names) for n in chain_names: self.chain.append(m3f.create_component(n)) self.proxy.subscribe_status(self.chain[-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.theta_desire_right_arm=[0]*self.bot.get_num_dof('right_arm') self.theta_desire_left_arm=[0]*self.bot.get_num_dof('left_arm') self.theta_desire_torso=[0]*self.bot.get_num_dof('torso') self.theta_desire_head=[0]*self.bot.get_num_dof('head') self.stiffness_right_arm=[0]*self.bot.get_num_dof('right_arm') self.stiffness_left_arm=[0]*self.bot.get_num_dof('left_arm') self.stiffness_torso=[0]*self.bot.get_num_dof('torso') self.stiffness_head=[0]*self.bot.get_num_dof('head') self.stiffness=[0] #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('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=3) self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3) 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('M3GuiSliders','Torque (mNm)', (self,'tq_desire'),range(len(self.bot.right_arm_ndof)),[-8000,8000],m3g.M3GuiWrite) #self.gui.add('M3GuiSliders','Pwm', (self,'pwm_desire'),range(len(self.sea_joint)),[-3200,3200],m3g.M3GuiWrite) #self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(1),[0,100],m3g.M3GuiWrite,column=1) #self.gui.add('M3GuiSliders','Slew ', (self,'slew'),range(0),[0,100],m3g.M3GuiWrite,column=3) self.gui.add('M3GuiSliders','Theta RA (Deg)', (self,'theta_desire_right_arm'),range(len(self.theta_desire_right_arm)),[-45,140],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Theta LA (Deg)', (self,'theta_desire_left_arm'),range(len(self.theta_desire_left_arm)),[-45,140],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Theta T (Deg)', (self,'theta_desire_torso'),range(len(self.theta_desire_torso)),[-45,140],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Theta H (Deg)', (self,'theta_desire_head'),range(len(self.theta_desire_head)),[-45,140],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Stiffness RA ', (self,'stiffness_right_arm'),range(len(self.stiffness_right_arm)),[0,100],m3g.M3GuiWrite,column=2) self.gui.add('M3GuiSliders','Stiffness LA )', (self,'stiffness_left_arm'),range(len(self.stiffness_left_arm)),[0,100],m3g.M3GuiWrite,column=2) self.gui.add('M3GuiSliders','Stiffness T ', (self,'stiffness_torso'),range(len(self.stiffness_torso)),[0,100],m3g.M3GuiWrite,column=2) #self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.sea_joint)),[0,100],m3g.M3GuiWrite,column=3) #self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite) self.gui.start(self.step)
#!/usr/bin/python ''' Created on Apr 28, 2014 @author: Antoine Hoarau ([email protected]) ''' import time import m3.rt_proxy as m3p import m3.humanoid as m3h import m3.toolbox as m3t if __name__ == '__main__': proxy = m3p.M3RtProxy() proxy.start() bot = m3h.M3Humanoid(m3t.get_robot_name()) bot.initialize(proxy) cnt=0 start = time.time() while 1: try: cnt=cnt+1 for chain in bot.get_available_chains(): bot.set_mode_theta_gc(chain) bot.set_slew_rate_proportion(chain,[1.0]*bot.get_num_dof(chain)) bot.set_stiffness(chain, [1.0]*bot.get_num_dof(chain)) bot.set_theta_deg(chain, [0.0]*bot.get_num_dof(chain)) if(time.time()-start >= 1): for chain in bot.get_available_chains(): print chain,bot.get_theta_deg(chain) print 'freq : ',cnt cnt=0 start = time.time()
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()
def start(self): # ######## Setup Proxy and Components ######################### self.proxy.start() self.current_first = True bot_name=m3t.get_robot_name() if bot_name == "": print 'Error: no robot components found:', bot_names return self.bot=m3.humanoid.M3Humanoid(bot_name) arm_names = self.bot.get_available_chains() arm_names = [x for x in arm_names if x.find('arm')!=-1] if len(arm_names)==0: print 'No arms found' return if len(arm_names)==1: self.arm_name=arm_names[0] else: self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0] # ####### Setup Hand ############# hand_names = self.bot.get_available_chains() hand_names = [x for x in hand_names if x.find('hand')!=-1] if len(hand_names)==0: print 'No hands found' return if len(arm_names)==1: self.hand_name=hand_names[0] else: self.hand_name = m3t.user_select_components_interactive(hand_names,single=True)[0] # ####### Setup Proxy ############# self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot) self.proxy.make_operational_all() self.bot.set_motor_power_on() self.ndof=self.bot.get_num_dof(self.arm_name) humanoid_shm_names=self.proxy.get_available_components('m3humanoid_shm') if len(humanoid_shm_names) > 0: self.proxy.make_safe_operational(humanoid_shm_names[0]) self.via_traj={} self.via_traj_first=True self.theta_curr = [0.0]*self.ndof # ######## Square/Circle stuff ######################### if self.arm_name == 'right_arm': self.center = [0.450, -0.28, -0.1745] else: self.center = [0.450, 0.28, -0.1745] avail_chains = self.bot.get_available_chains() for c in avail_chains: if c == 'torso': self.center[2] += 0.5079 self.jt = m3jt.JointTrajectory(7) self.axis_demo = [0, 0, 1] # ##### Generate square vias ############################ '''ik_vias=[] length_m = 25/ 100.0 resolution = 20 y_left = self.center[1] + length_m/2.0 y_right = self.center[1] - length_m/2.0 z_top = self.center[2] + length_m/2.0 z_bottom = self.center[2] - length_m/2.0 dy = (y_left - y_right) / nu.float(resolution) dz = (z_top - z_bottom) / nu.float(resolution) x = self.center[0] if self.arm_name=='right_arm': # first add start point ik_vias.append([x, y_left, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add top line for i in range(resolution): ik_vias.append([x, y_left - (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]]) # add right line for i in range(resolution): ik_vias.append([x, y_right, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add bottom line for i in range(resolution): ik_vias.append([x, y_right + (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add left line for i in range(resolution): ik_vias.append([x, y_left, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) else: # first add start point ik_vias.append([x, y_right, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add top line for i in range(resolution): ik_vias.append([x, y_right + (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]]) # add right line for i in range(resolution): ik_vias.append([x, y_left, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add bottom line for i in range(resolution): ik_vias.append([x, y_left - (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add left line for i in range(resolution): ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) self.via_traj['Square']=[] # use zero position as reference for IK solver self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name)) for ikv in ik_vias: theta_soln = [] #print 'solving for square ik via:', ikv if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln): self.via_traj['Square'].append(theta_soln) self.bot.set_theta_sim_deg(self.arm_name,theta_soln) else: print 'WARNING: no IK solution found for via ', ikv self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name)) ''' # ##### Generate circle vias ############################ ik_vias=[] diameter_m = 25.0 / 100.0 resolution = 40 x = self.center[0] for i in range(resolution*4 + 1): dt = 2*nu.pi/(nu.float(resolution)*4) t = (nu.pi/2) + i*dt if t > nu.pi: t -= 2*nu.pi y = self.center[1] + (diameter_m/2.0) * nu.cos(t) z = self.center[2] + (diameter_m/2.0) * nu.sin(t) ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) self.via_traj['Circle']=[] # use zero position as reference for IK solver self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name)) for ikv in ik_vias: theta_soln = [] if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln): self.via_traj['Circle'].append(theta_soln) self.bot.set_theta_sim_deg(self.arm_name,theta_soln) else: print 'WARNING: no IK solution found for via ', ikv self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name)) # ##### Load Via Trajectories ############################ self.via_files={'TrajA':'kha1.via'} for k in self.via_files.keys(): fn=m3t.get_animation_file(self.via_files[k]) print "Loading:",fn try: with open(fn,'r') as f: d=yaml.safe_load(f.read()) self.via_traj[k]=d[self.arm_name] except IOError: print 'Via file',k,'not present. Skipping...' # ##### Hand Trajectories ############################ if self.hand_name is not None: try: with open(m3t.get_animation_file(self.hand_name+'_postures.yml'),'r') as f: self.hand_data= yaml.safe_load(f.read()) except Exception,e: print e
def start(self): # ######## Setup Proxy and Components ######################### self.proxy.start() bot_name = m3t.get_robot_name() if bot_name == "": print "Error: no robot components found:", bot_names return self.bot = m3.humanoid.M3Humanoid(bot_name) arm_names = self.bot.get_available_chains() arm_names = [x for x in arm_names if x.find("arm") != -1] if len(arm_names) == 0: print "No arms found" return if len(arm_names) == 1: self.arm_name = arm_names[0] else: self.arm_name = m3t.user_select_components_interactive(arm_names, single=True)[0] # ####### Setup Hand ############# hand_names = self.proxy.get_available_components("m3hand") hand_name = "" if len(hand_names) > 1: hand_name = m3t.user_select_components_interactive(chain_names, single=True) if len(hand_names) == 1: hand_name = hand_names[0] if len(hand_name): self.hand = m3.hand.M3Hand(hand_name) self.proxy.publish_command(self.hand) self.proxy.subscribe_status(self.hand) else: self.hand = None # ####### Setup Proxy ############# self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot) self.proxy.make_operational_all() self.bot.set_motor_power_on() self.ndof = self.bot.get_num_dof(self.arm_name) self.via_traj = {} self.via_traj_first = True # ######## Demo and GUI ######################### self.off = False self.grasp = False self.hammer_up = False self.switch_start = True self.task_mode_names = ["Off", "Zero", "HammerGrasp", "Hammer"] self.arm_mode_methods = [self.step_off, self.step_zero, self.step_hammer_grasp, self.step_hammer] self.hand_mode_methods = [self.step_hand_off, self.step_hand_off, self.step_hand_grasp, self.step_hand_grasp] self.task_mode = [0] self.poses = { "zero": {"right_arm": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}, "hammer_grasp": {"right_arm": [33, 0, 0, 120.18858337402344, 0, -60, 0]}, "hammer_up": {"right_arm": [33, 0, 0, 120.18858337402344, 0, -60, 0]}, "hammer_down": {"right_arm": [33, 0, 0, 71.249755859375, 0, -60, 0]}, } self.stiffness = [100] self.velocity = [25] self.cycle_time = [3000] self.gui.add( "M3GuiModes", "TaskMode", (self, "task_mode"), range(1), [self.task_mode_names, 1], m3g.M3GuiWrite, column=1 ) if self.hand is not None: self.gui.add( "M3GuiToggle", "Grasp", (self, "grasp"), [], [["GraspOpen", "GraspClosed"]], m3g.M3GuiWrite, column=1 ) self.gui.add("M3GuiSliders", "Stiffness ", (self, "stiffness"), [0], [0, 100], m3g.M3GuiWrite, column=1) self.gui.add("M3GuiSliders", "Velocity ", (self, "velocity"), [0], [0, 40], m3g.M3GuiWrite, column=1) self.gui.add("M3GuiSliders", "CycleTimeMs", (self, "cycle_time"), [0], [0, 4000], m3g.M3GuiWrite, column=1) 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] # ####### Setup Hand ############# hand_names=self.proxy.get_available_components('m3hand') hand_name='' if len(hand_names)>1: hand_name=m3t.user_select_components_interactive(chain_names,single=True) if len(hand_names)==1: hand_name=hand_names[0] if len(hand_name): self.hand=m3.hand.M3Hand(hand_name) self.proxy.publish_command(self.hand) self.proxy.subscribe_status(self.hand) else: self.hand=None # ####### Setup Proxy ############# self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot) self.proxy.make_operational_all() self.bot.set_motor_power_on() self.ndof=self.bot.get_num_dof(self.arm_name) self.via_traj={} self.via_traj_first=True self.theta_curr = [0.0]*self.ndof # ######## Square/Circle stuff ######################### if self.arm_name == 'right_arm': self.center = [0.450, -0.28, -0.1745] else: self.center = [0.450, 0.28, -0.1745] avail_chains = self.bot.get_available_chains() for c in avail_chains: if c == 'torso': self.center[2] += 0.5079 self.jt = m3jt.JointTrajectory(7) self.axis_demo = [0, 0, 1] # ##### Generate square vias ############################ ik_vias=[] length_m = 25/ 100.0 resolution = 20 y_left = self.center[1] + length_m/2.0 y_right = self.center[1] - length_m/2.0 z_top = self.center[2] + length_m/2.0 z_bottom = self.center[2] - length_m/2.0 dy = (y_left - y_right) / nu.float(resolution) dz = (z_top - z_bottom) / nu.float(resolution) x = self.center[0] if self.arm_name=='right_arm': # first add start point ik_vias.append([x, y_left, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add top line for i in range(resolution): ik_vias.append([x, y_left - (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]]) # add right line for i in range(resolution): ik_vias.append([x, y_right, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add bottom line for i in range(resolution): ik_vias.append([x, y_right + (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add left line for i in range(resolution): ik_vias.append([x, y_left, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) else: # first add start point ik_vias.append([x, y_right, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add top line for i in range(resolution): ik_vias.append([x, y_right + (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]]) # add right line for i in range(resolution): ik_vias.append([x, y_left, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add bottom line for i in range(resolution): ik_vias.append([x, y_left - (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) # add left line for i in range(resolution): ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) self.via_traj['Square']=[] # use zero position as reference for IK solver self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name)) for ikv in ik_vias: theta_soln = [] #print 'solving for square ik via:', ikv if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln): self.via_traj['Square'].append(theta_soln) self.bot.set_theta_sim_deg(self.arm_name,theta_soln) else: print 'WARNING: no IK solution found for via ', ikv self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name)) # ##### Generate circle vias ############################ ik_vias=[] diameter_m = 25.0 / 100.0 resolution = 20 x = self.center[0] for i in range(resolution*4 + 1): dt = 2*nu.pi/(nu.float(resolution)*4) t = (nu.pi/2) + i*dt if t > nu.pi: t -= 2*nu.pi y = self.center[1] + (diameter_m/2.0) * nu.cos(t) z = self.center[2] + (diameter_m/2.0) * nu.sin(t) ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]]) self.via_traj['Circle']=[] # use zero position as reference for IK solver self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name)) for ikv in ik_vias: theta_soln = [] if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln): self.via_traj['Circle'].append(theta_soln) self.bot.set_theta_sim_deg(self.arm_name,theta_soln) else: print 'WARNING: no IK solution found for via ', ikv self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name)) # ##### Load Via Trajectories ############################ self.via_files={'TrajA':'kha1.via'} pt=m3t.get_m3_animation_path() for k in self.via_files.keys(): fn=pt+self.via_files[k] try: f=file(fn,'r') d=yaml.safe_load(f.read()) self.via_traj[k]=d[self.arm_name] except IOError: print 'Via file',k,'not present. Skipping...' # ##### Hand Trajectories ############################ if self.hand is not None: pf=m3t.get_m3_animation_path()+hand_name+'_postures.yml' f=file(pf,'r') self.hand_data= yaml.safe_load(f.read()) f.close() self.hand_traj_first=True # ######## Demo and GUI ######################### self.off=False self.grasp=False self.arm_mode_names=['Off','Zero','Current','HoldUp','Square','Circle','TrajA'] self.hand_mode_names=['Off','Open','Grasp','Animation'] self.arm_mode_methods=[self.step_off,self.step_zero,self.step_current,self.step_hold_up,self.step_via_traj, self.step_via_traj,self.step_via_traj] self.hand_mode_methods=[self.step_hand_off,self.step_hand_open,self.step_hand_grasp,self.step_hand_animation] self.arm_mode=[0] self.hand_mode=[0] self.poses={'zero': {'right_arm':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],'left_arm':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}, 'holdup':{'right_arm':[56.0, 26.0, -8.0, 84.0, 119.0, -36.0, 2.0],'left_arm':[56.0, -26.0, 8.0, 84.0, -119.0, -36.0, -2.0]}} self.stiffness=[50] self.velocity=[25] self.gui.add('M3GuiModes', 'Arm Mode', (self,'arm_mode'),range(1),[self.arm_mode_names,1],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiToggle', 'ESTOP', (self,'off'),[],[['Arm Enabled','Arm Disabled']],m3g.M3GuiWrite,column=1) if self.hand is not None: self.gui.add('M3GuiToggle', 'Grasp', (self,'grasp'),[],[['GraspOpen','GraspClosed']],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiModes', 'Hand Mode', (self,'hand_mode'),range(1),[self.hand_mode_names,1],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),[0],[0,100],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Velocity ', (self,'velocity'),[0],[0,40],m3g.M3GuiWrite,column=1) self.gui.start(self.step)
proxy.start() proxy.make_operational_all() print '--------------------------' print 'Note: RVIZ support is only intended for debugging in the demo.' print ' Motor power should be turned off if using RVIZ.' 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()
'postures': vias[c], 'param': param } #safe defaults f.write(yaml.safe_dump(d)) vias = {} for c in self.chains: vias[c] = [] if k == 'q': self.done = True # ###################################################### proxy = m3p.M3RtProxy() proxy.start() bot_name = m3t.get_robot_name() if bot_name == "": print 'Error: no robot components found:', bot_name exit() 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() proxy.step() chains = bot.get_available_chains() print 'Select chains to pose' chains = m3t.user_select_components_interactive(chains) menu = menu_thread(bot, chains)