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 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 = []
rviz = False
pub = None
if k == 'y':
rviz = True
bot_name = m3t.get_robot_name()
if bot_name == "":
print 'Error: no botanoid components found:', bot_names
proxy.stop()
sys.exit()
bot = m3f.create_component(bot_name)
viz = None
if rviz == True:
viz = m3v.M3Viz(proxy, bot)
proxy.publish_param(bot) #allow to set payload
proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
humanoid_shm_names = proxy.get_available_components('m3humanoid_shm')
if len(humanoid_shm_names) > 0:
proxy.make_safe_operational(humanoid_shm_names[0])
if not rviz == True:
bot.set_motor_power_on()
proxy.step()
if rviz == True:
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)