def load_postures():
fn=m3t.get_m3_animation_path()+'demo_head_h1_postures.yml'
try:
f=file(fn,'r')
postures = yaml.safe_load(f.read())
f.close()
except IOError:
print 'Posture file not present'
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 load_animation(self, anim_name):
"""Call with each animation to load at startup"""
ani_paths = m3t.get_m3_animation_path()
for ani_path in ani_paths:
filename = ani_path + "mouth/" + anim_name + ".yml"
try:
f=file(filename,'r')
self.animations[anim_name]= yaml.safe_load(f.read())
except (IOError, EOFError):
print 'Animation file not present:',filename
for k in self.animations[anim_name]['sequence']:
filename = ani_path + "/mouth/" + k['file']
try:
k['image']= Image.open(filename)
except (IOError, EOFError):
print 'File',filename,'not found'
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):
# ######## 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 get_available_images(self):
all_img= [glob.glob(p + "mouth/*.tiff") for p in m3t.get_m3_animation_path()]
return [item for sublist in all_img for item in sublist]
def start(self):
self.proxy.start()
self.proxy.make_operational_all()
chain_names=self.proxy.get_available_components('m3hand')
if len(chain_names)>1:
hand_name=m3t.user_select_components_interactive(chain_names,single=True)
else:
hand_name=chain_names
pwr_name=self.proxy.get_available_components('m3pwr')
if len(pwr_name)>1:
pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)
print 'Position arm [y]?'
if m3t.get_yes_no('y'):
arm_names=self.proxy.get_available_components('m3arm')
if len(arm_names)>1:
print 'Select arm: '
arm_name=m3t.user_select_components_interactive(arm_names,single=True)[0]
else:
arm_name=arm_names[0]
self.arm=m3f.create_component(arm_name)
self.proxy.publish_command(self.arm)
self.arm.set_mode_theta_gc()
self.arm.set_theta_deg([30,0,0,110,0,0,0])
self.arm.set_stiffness(0.5)
self.arm.set_slew_rate_proportion([0.75]*7)
self.chain=m3f.create_component(hand_name[0])
self.proxy.publish_command(self.chain)
self.proxy.subscribe_status(self.chain)
self.pwr=m3f.create_component(pwr_name[0])
self.proxy.publish_command(self.pwr)
self.pwr.set_motor_power_on()
#Force safe-op of robot if present
hum=self.proxy.get_available_components('m3humanoid')
if len(hum)>0:
self.proxy.make_safe_operational(hum[0])
#Setup postures
self.posture_filename=m3t.get_m3_animation_path()+self.chain.name+'_postures.yml'
f=file(self.posture_filename,'r')
self.data= yaml.safe_load(f.read())
self.param=self.data['param']
f.close()
self.theta_desire=[0,0,0,0,0]
self.mode=[1,1,1,1,1]
#Create gui
self.run=False
self.run_last=False
self.running=False
self.grasp=False
self.grasp_last=False
self.grasp_off=False
self.grasp_off_ts=time.time()
self.status_dict=self.proxy.get_status_dict()
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=3)
self.gui.add('M3GuiTree', 'Param', (self,'param'),[],[],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiToggle', 'Animation', (self,'run'),[],[['Run','Stop']],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiModes', 'Joint', (self,'mode'),range(5),[['Off','Enabled'],1],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiSliders','Theta (Deg)', (self,'theta_desire'),range(5),[0,300],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiToggle', 'Power Grasp', (self,'grasp'),[],[['Run','Stop']],m3g.M3GuiWrite,column=2)
self.gui.start(self.step)
def start(self):
self.proxy.start()
self.proxy.make_operational_all()
chain_names = self.proxy.get_available_components('m3hand')
if len(chain_names) > 1:
hand_name = m3t.user_select_components_interactive(chain_names,
single=True)
else:
hand_name = chain_names
pwr_name = self.proxy.get_available_components('m3pwr')
if len(pwr_name) > 1:
pwr_name = m3t.user_select_components_interactive(pwr_name,
single=True)
print 'Position arm [y]?'
if m3t.get_yes_no('y'):
arm_names = self.proxy.get_available_components('m3arm')
if len(arm_names) > 1:
print 'Select arm: '
arm_name = m3t.user_select_components_interactive(
arm_names, single=True)[0]
else:
arm_name = arm_names[0]
self.arm = m3f.create_component(arm_name)
self.proxy.publish_command(self.arm)
self.arm.set_mode_theta_gc()
self.arm.set_theta_deg([30, 0, 0, 110, 0, 0, 0])
self.arm.set_stiffness(0.5)
self.arm.set_slew_rate_proportion([0.75] * 7)
self.chain = m3f.create_component(hand_name[0])
self.proxy.publish_command(self.chain)
self.proxy.subscribe_status(self.chain)
self.pwr = m3f.create_component(pwr_name[0])
self.proxy.publish_command(self.pwr)
self.pwr.set_motor_power_on()
#Force safe-op of robot if present
hum = self.proxy.get_available_components('m3humanoid')
if len(hum) > 0:
self.proxy.make_safe_operational(hum[0])
#Setup postures
self.posture_filename = m3t.get_m3_animation_path(
) + self.chain.name + '_postures.yml'
f = file(self.posture_filename, 'r')
self.data = yaml.safe_load(f.read())
self.param = self.data['param']
f.close()
self.theta_desire = [0, 0, 0, 0, 0]
self.mode = [1, 1, 1, 1, 1]
#Create gui
self.run = False
self.run_last = False
self.running = False
self.grasp = False
self.grasp_last = False
self.grasp_off = False
self.grasp_off_ts = time.time()
self.status_dict = self.proxy.get_status_dict()
self.gui.add('M3GuiTree',
'Status', (self, 'status_dict'), [], [],
m3g.M3GuiRead,
column=3)
self.gui.add('M3GuiTree',
'Param', (self, 'param'), [], [],
m3g.M3GuiWrite,
column=3)
self.gui.add('M3GuiToggle',
'Animation', (self, 'run'), [], [['Run', 'Stop']],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiModes',
'Joint', (self, 'mode'),
range(5), [['Off', 'Enabled'], 1],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiSliders',
'Theta (Deg)', (self, 'theta_desire'),
range(5), [0, 300],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiToggle',
'Power Grasp', (self, 'grasp'), [], [['Run', 'Stop']],
m3g.M3GuiWrite,
column=2)
self.gui.start(self.step)
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)
menu.start()
slew = {}
qdes = {}
fn = m3t.get_m3_animation_path() + m3t.get_robot_name() + '_poser_config.yml'
f = file(fn, 'r')
config = yaml.safe_load(f.read())
f.close()
stiffness = config['stiffness']
delta_thresh = config['delta_thresh']
slew_rate = config['slew_rate']
proxy.step()
dbg = ''
for c in chains:
ndof = bot.get_num_dof(c)
slew[c] = []
qdes[c] = []
for i in range(ndof):
slew[c].append(m3t.M3Slew())
qdes[c] = nu.array(bot.get_theta_deg(c), nu.Float)
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.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)
menu.start()
slew = {}
qdes = {}
fn = m3t.get_m3_animation_path() + m3t.get_robot_name() + "_poser_config.yml"
f = file(fn, "r")
config = yaml.safe_load(f.read())
f.close()
stiffness = config["stiffness"]
delta_thresh = config["delta_thresh"]
slew_rate = config["slew_rate"]
proxy.step()
dbg = ""
for c in chains:
ndof = bot.get_num_dof(c)
slew[c] = []
qdes[c] = []
for i in range(ndof):
slew[c].append(m3t.M3Slew())
qdes[c] = nu.array(bot.get_theta_deg(c), nu.Float)
