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 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
import m3.humanoid import m3.toolbox as m3t import yaml proxy = m3p.M3RtProxy() proxy.start() bot_name = m3t.get_robot_name() bot = m3.humanoid.M3Humanoid(bot_name) proxy.subscribe_status(bot) vias = [] print "Hit <SPACE> to record right arm posture, <Q> to save postures and quit" while True: key = m3t.get_keystroke() if key == ' ': proxy.step() t = bot.get_theta_deg('right_arm') vias.append(m3t.float_list(t)) print 'Record of: ', vias[-1] if key == 'q': fn = 'poses.yml' f = file(fn, 'w') f.write(yaml.safe_dump(vias)) print 'File saved as: ', fn break