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()
base_name=proxy.get_available_components('m3omnibase')
if len(base_name)!=1:
print 'Omnibase not found. Proceeding...'
else:
omni=m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
if omni==None and zlift==None:
exit()
if zlift is not None:
pwr_rt=m3t.get_joint_pwr_component_name(zlift_names[0])
else:
pwr_rt=m3t.get_omnibase_pwr_component_name(base_name[0])
pwr=m3f.create_component(pwr_rt)
proxy.publish_command(pwr)
pwr.set_motor_power_on()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()
zlift_shm_names=proxy.get_available_components('m3joint_zlift_shm')
if len(zlift_shm_names) > 0:
proxy.make_safe_operational(zlift_shm_names[0])
omnibase_shm_names=proxy.get_available_components('m3omnibase_shm')
if len(omnibase_shm_names) > 0:
base_name = proxy.get_available_components('m3omnibase')
if len(base_name) != 1:
print 'Omnibase not found. Proceeding...'
else:
omni = m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
if omni == None and zlift == None:
exit()
if zlift is not None:
pwr_rt = m3t.get_joint_pwr_component_name(zlift_names[0])
else:
pwr_rt = m3t.get_omnibase_pwr_component_name(base_name[0])
pwr = m3f.create_component(pwr_rt)
proxy.publish_command(pwr)
pwr.set_motor_power_on()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()
zlift_shm_names = proxy.get_available_components('m3joint_zlift_shm')
if len(zlift_shm_names) > 0:
proxy.make_safe_operational(zlift_shm_names[0])
omnibase_shm_names = proxy.get_available_components('m3omnibase_shm')
if len(omnibase_shm_names) > 0:
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 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')
if len(base_name)!=1:
print 'Invalid number of base components available'
proxy.stop()
exit()
omni=m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])]
#proxy.get_available_components('m3pwr')
#if len(pwr_name)>1:
#pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)
pwr=m3f.create_component(pwr_name[0])
proxy.subscribe_status(pwr)
proxy.publish_command(pwr)
proxy.make_operational(pwr_name[0])
proxy.step()
omni.set_mode_off()
pwr.set_motor_power_on()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()
omni.calibrate(proxy)
time.sleep(0.5)
print "Turn power on to robot and press any key."
raw_input()
omni.set_local_position(0,0,0,proxy)
omni.set_global_position(0,0,0,proxy)
omni.set_max_linear_accel(max_lin_acc)
omni.set_max_linear_velocity(max_lin_vel)
omni.set_max_rotation_velocity(max_rot_vel)
omni.set_max_rotation_accel(max_rot_acc)
proxy.step()
'''p = omni.get_global_position()
print 'Pos:', p'''
omni.set_mode_traj_goal()
omni.set_traj_goal(0, 0, 0)
proxy.step()
time.sleep(4)
print 'Bus voltage:',omni.get_bus_voltage()
print 'Press any key to begin pacing.'
raw_input()
try:
while True:
omni.set_traj_goal(2.0, 0, 180)
proxy.step()
time.sleep(0.1)
proxy.step()
while not omni.is_traj_goal_reached():
proxy.step()
p = omni.get_global_position()
print 'Pos:', p
time.sleep(0.1)
omni.set_traj_goal(0, 0, 0)
proxy.step()
time.sleep(0.1)
proxy.step()
while not omni.is_traj_goal_reached():
proxy.step()
p = omni.get_global_position()
print 'Pos:', p
time.sleep(0.1)
except KeyboardInterrupt:
pass
omni.set_mode_off()
pwr.set_motor_power_off()
proxy.step()
proxy.stop()
def start(self):
self.proxy.start()
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)
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)
