def main(): proxy = m3p.M3RtProxy() proxy.start() base_name = proxy.get_available_components('m3omnibase') if len(base_name) != 1: print 'Invalid number of base components available' proxy.stop() exit() omni = m3o.M3OmniBase(base_name[0]) proxy.publish_param(omni) # we need this for calibration proxy.subscribe_status(omni) proxy.publish_command(omni) pwr_name = [m3t.get_omnibase_pwr_component_name(base_name[0])] #proxy.get_available_components('m3pwr') #if len(pwr_name)>1: #pwr_name=m3t.user_select_components_interactive(pwr_name,single=True) pwr = m3f.create_component(pwr_name[0]) proxy.subscribe_status(pwr) proxy.publish_command(pwr) proxy.make_operational(pwr_name[0]) proxy.step() omni.set_mode_off() pwr.set_motor_power_on() proxy.make_operational_all() proxy.step() time.sleep(0.5) proxy.step() omni.calibrate(proxy) time.sleep(0.5) print "Turn power on to robot and press any key." raw_input() omni.set_local_position(0, 0, 0, proxy) omni.set_global_position(0, 0, 0, proxy) omni.set_max_linear_accel(max_lin_acc) omni.set_max_linear_velocity(max_lin_vel) omni.set_max_rotation_velocity(max_rot_vel) omni.set_max_rotation_accel(max_rot_acc) proxy.step() '''p = omni.get_global_position() print 'Pos:', p''' omni.set_mode_traj_goal() omni.set_traj_goal(0, 0, 0) proxy.step() time.sleep(4) print 'Bus voltage:', omni.get_bus_voltage() print 'Press any key to begin pacing.' raw_input() try: while True: omni.set_traj_goal(2.0, 0, 180) proxy.step() time.sleep(0.1) proxy.step() while not omni.is_traj_goal_reached(): proxy.step() p = omni.get_global_position() print 'Pos:', p time.sleep(0.1) omni.set_traj_goal(0, 0, 0) proxy.step() time.sleep(0.1) proxy.step() while not omni.is_traj_goal_reached(): proxy.step() p = omni.get_global_position() print 'Pos:', p time.sleep(0.1) except KeyboardInterrupt: pass omni.set_mode_off() pwr.set_motor_power_off() proxy.step() proxy.stop()
proxy = m3p.M3RtProxy() proxy.start() pwr_name = proxy.get_available_components('m3pwr') if len(pwr_name) > 1: pwr_name = m3t.user_select_components_interactive(pwr_name, single=True) pwr = m3rt.M3Pwr(pwr_name[0]) proxy.subscribe_status(pwr) base_name = proxy.get_available_components('m3omnibase') if len(base_name) != 1: print 'Invalid number of base components available' proxy.stop() exit() omni = m3o.M3OmniBase(base_name[0]) proxy.subscribe_status(omni) scope_steer = m3t.M3ScopeN(xwidth=100, title='Steer and Total Currents') scope_roll = m3t.M3ScopeN(xwidth=100, title='Roll Currents') try: ts = time.time() while True: proxy.step() motor_current = omni.get_motor_torques() roll_current = [] str_current = [] for i in range(4): roll_current.append(motor_current[i * 2]) str_current.append(motor_current[i * 2 + 1])
def main(): proxy = m3p.M3RtProxy() proxy.start() base_name=proxy.get_available_components('m3omnibase') if len(base_name)!=1: print 'Invalid number of base components available' proxy.stop() exit() omni=m3o.M3OmniBase(base_name[0]) proxy.publish_param(omni) # we need this for calibration proxy.subscribe_status(omni) proxy.publish_command(omni) pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])] #proxy.get_available_components('m3pwr') #if len(pwr_name)>1: #pwr_name=m3t.user_select_components_interactive(pwr_name,single=True) pwr=m3f.create_component(pwr_name[0]) proxy.subscribe_status(pwr) proxy.publish_command(pwr) proxy.make_operational(pwr_name[0]) proxy.step() pwr.set_motor_power_on() proxy.step() proxy.make_operational_all() proxy.step() time.sleep(0.5) proxy.step() omni.calibrate(proxy) time.sleep(0.5) omni.set_local_position(0,0,0,proxy) omni.set_global_position(0,0,0,proxy) omni.set_mode_traj_via() '''omni.add_via(0, 1.2, -180) omni.add_via(1, 1.2, 0) omni.add_via(1, 0, 180) omni.add_via(0, 0, 0)''' omni.add_via(0, 0.2, 0) omni.add_via(0.2, 0.2, 0) omni.add_via(0.2, 0, 0) omni.add_via(0, 0, 0) proxy.step() time.sleep(0.2) proxy.step() '''while not omni.is_traj_goal_reached(): proxy.step()''' try: while True: proxy.step() p = omni.get_global_position() d = omni.get_desired_position() print '-----------Local Pos-------' print 'X:', p[0] print 'Y:', p[1] print 'Yaw:', p[2] print '---------------------------' print '-----------Desired Pos-------' print 'dX:', d[0] print 'dY:', d[1] print 'dYaw:', d[2] print '---------------------------' time.sleep(0.1) except (KeyboardInterrupt): pass stop()
def main(): proxy = m3p.M3RtProxy() proxy.start() base_name=proxy.get_available_components('m3omnibase') omni=None if len(base_name)!=1: print 'Omnibase not available. Proceeding without it...' else: print 'Use OmniBase [y]?' if m3t.get_yes_no('y'): omni=m3o.M3OmniBase(base_name[0]) proxy.publish_param(omni) # we need this for calibration proxy.subscribe_status(omni) proxy.publish_command(omni) pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])] #proxy.get_available_components('m3pwr') #if len(pwr_name)>1: #pwr_name=m3t.user_select_components_interactive(pwr_name,single=True) pwr=m3f.create_component(pwr_name[0]) proxy.subscribe_status(pwr) proxy.publish_command(pwr) zlift_names=proxy.get_available_components('m3joint_zlift') zl=None if len(zlift_names)==1: print 'Use Zlift [y]?' if m3t.get_yes_no('y'): zl=m3z.M3JointZLift(zlift_names[0]) proxy.subscribe_status(zl) proxy.publish_command(zl) proxy.publish_param(zl) proxy.make_operational(pwr_name[0]) proxy.step() if omni is not None: omni.set_mode_off() pwr.set_motor_power_on() proxy.make_operational_all() zlift_shm_names=proxy.get_available_components('m3joint_zlift_shm') if len(zlift_shm_names) > 0: proxy.make_safe_operational(zlift_shm_names[0]) omnibase_shm_names=proxy.get_available_components('m3omnibase_shm') if len(omnibase_shm_names) > 0: proxy.make_safe_operational(omnibase_shm_names[0]) humanoid_shm_names=proxy.get_available_components('m3humanoid_shm') if len(humanoid_shm_names) > 0: proxy.make_safe_operational(humanoid_shm_names[0]) m3ledx2xn_ec_shm_names=proxy.get_available_components('m3ledx2xn_ec_shm') if len(m3ledx2xn_ec_shm_names) > 0: proxy.make_safe_operational(m3ledx2xn_ec_shm_names[0]) m3led_matrix_ec_shm_names=proxy.get_available_components('m3led_matrix_ec_shm') if len(m3led_matrix_ec_shm_names) > 0: proxy.make_safe_operational(m3led_matrix_ec_shm_names[0]) proxy.step() time.sleep(0.5) if omni is not None: proxy.step() omni.calibrate(proxy) time.sleep(0.5) if zl is not None: if not zl.calibrate(proxy): zl=None print 'ZLift failed to calibrate' if omni is None and zl is None: exit() print "Turn motor power on to Omnibase and press any key." raw_input() joy=m3to.M3OmniBaseJoy() joy.start(proxy,omni,zl) k = 0 try: while True: joy.step() #print 'Bus Current:', pwr.get_bus_torque() p = omni.get_local_position() #omni.set_op_space_forces(f.jx*200.0, f.jy*200.0, f.jyaw*50.0) k += 1 if k == 100: print '-----------Local Pos-------' print 'X:', p[0] print 'Y:', p[1] print 'Yaw:', p[2] #print '---------------------------' k = 0 '''print '-------Joystick Pos-------' print 'jx:',f.jx print 'jy:', f.jy print 'jyaw:', f.jyaw print 'button:', f.jbutton print '---------------------------' #print 'Bus voltage',omni.get_bus_voltage()''' '''tqs = omni.get_motor_torques() print tqs[0], tqs[2], tqs[4], tqs[6]''' #print omni.get_steer_torques() proxy.step() except KeyboardInterrupt: pass joy.stop() if omni is not None: omni.set_mode_off() pwr.set_motor_power_off() proxy.step() proxy.stop()
def start(self): self.proxy.start() 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]) #Setup Components base_name = self.proxy.get_available_components('m3omnibase') if len(base_name) != 1: print 'Invalid number of base components available' self.proxy.stop() exit() self.omni = m3o.M3OmniBase(base_name[0]) print 'Base name =' + base_name[0] self.proxy.publish_param(self.omni) self.proxy.subscribe_status(self.omni) self.proxy.publish_command(self.omni) pwr_name = [m3t.get_omnibase_pwr_component_name(base_name[0])] print "pwr_name :", pwr_name self.pwr = m3f.create_component(pwr_name[0]) self.proxy.subscribe_status(self.pwr) self.proxy.publish_command(self.pwr) self.proxy.make_operational(pwr_name[0]) self.proxy.step() self.pwr.set_motor_power_on() self.proxy.make_operational_all() self.proxy.step() self.omni.calibrate(self.proxy) #Create gui self.ctrl_mode = [0] self.traj_mode = [0] self.joy_button = [0] self.local_velocity_desired_x = [0] self.local_velocity_desired_y = [0] self.local_velocity_desired_heading = [0] self.joy_x = [0] self.joy_y = [0] self.joy_yaw = [0] self.goal_x = [0] self.goal_y = [0] self.goal_yaw = [0] self.caster_ctrl_idx = [0] self.caster_ctrl_mode = [0] self.steer_or_roll_mode = [0] self.op_force_desired_x = [0] self.op_force_desired_y = [0] self.op_torque_desired = [0] self.joint_torque = [0] self.joint_vel = [0] self.save = False self.save_last = False self.init_g = self.omni.get_global_position() 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', 'CtrlMode', (self, 'ctrl_mode'), range(1), [[ 'Off', 'Calibrate', 'Caster', 'OpSpaceForce', 'OpSpaceTraj', 'CartLocal', 'CartGlob' ], 1], m3g.M3GuiWrite) self.gui.add('M3GuiModes', 'TrajMode', (self, 'traj_mode'), range(1), [['Off', 'Joystick', 'Goal', 'Vias'], 1], m3g.M3GuiWrite) self.gui.add('M3GuiModes', 'Caster Steer/Roll', (self, 'steer_or_roll_mode'), range(1), [['Steer', 'Roll'], 1], m3g.M3GuiWrite) self.gui.add('M3GuiModes', 'CasterCtrlIdx', (self, 'caster_ctrl_idx'), range(1), [['0', '1', '2', '3'], 1], m3g.M3GuiWrite) self.gui.add('M3GuiModes', 'CasterCtrlMode', (self, 'caster_ctrl_mode'), range(1), [['Off', 'Tq', 'Vel'], 1], m3g.M3GuiWrite) self.gui.add('M3GuiModes', 'JoyButton', (self, 'joy_button'), range(1), [['Tri', 'Circ', 'Sqr', 'X'], 1], m3g.M3GuiWrite) self.gui.add('M3GuiSliders', 'JoystickX', (self, 'joy_x'), range(1), [-1, 1], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'JoystickY', (self, 'joy_y'), range(1), [-1, 1], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'JoystickYaw', (self, 'joy_yaw'), range(1), [-1, 1], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'LocalVelX (m/s)', (self, 'local_velocity_desired_x'), range(1), [-1, 1], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'LocalVelY (m/s)', (self, 'local_velocity_desired_y'), range(1), [-1, 1], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'LocalVelHeading (deg/s)', (self, 'local_velocity_desired_heading'), range(1), [-60, 60], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'OpForceX (N)', (self, 'op_force_desired_x'), range(1), [-1000, 1000], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'OpForceY (N)', (self, 'op_force_desired_y'), range(1), [-1000, 1000], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'OpTorque (Nm)', (self, 'op_torque_desired'), range(1), [-50, 50], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'Jnt Torque (Nm)', (self, 'joint_torque'), range(1), [-12.8, 12.8], m3g.M3GuiWrite, column=1) # self.gui.add('M3GuiSliders','Jnt Torque (Nm)', (self,'joint_torque'),range(1),[-6.4,6.4],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders', 'Jnt Vel (deg/s)', (self, 'joint_vel'), range(1), [-90, 90], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'Goal X (m)', (self, 'goal_x'), range(1), [-2, 2], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'Goal Y (m)', (self, 'goal_y'), range(1), [-2, 2], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiSliders', 'Goal Yaw(deg)', (self, 'goal_yaw'), range(1), [-180, 180], m3g.M3GuiWrite, column=1) time.sleep(0.5) self.omni.set_local_position(0, 0, 0, self.proxy) self.omni.set_global_position(0, 0, 0, self.proxy) self.gui.start(self.step)