'l_ankle_y': 45., 'r_hip_z': -10., 'l_hip_z': 10., 'r_hip_x': 0., 'l_hip_x': 0., }) if key == "y": for m in p.motors: if m in [p.r_shoulder_y, p.l_shoulder_y]: # misbehaving shoulder crawl_angles.pop(m.name) m.compliant = True else: m.compliant = False p.goto_position(crawl_angles, 3, wait=True) wait_each = False movement_time = .5 repetitions = 4 pt.ion() fig = pt.figure() new_frame = c.frame.astype(float)[:, :, [2, 1, 0]] / 255. frame = new_frame pt.clf() ax1 = pt.subplot(1, 2, 1) pt.imshow(frame) ax1.set_title("Old vision") ax2 = pt.subplot(1, 2, 2)
raw_input('Ready to turn off compliance and lift out of bag?') for m in p.motors: m.compliant = False raw_input('Ready to make legs compliant and put into sit position?') for rl in 'rl': for m in ['ankle', 'knee', 'hip']: getattr(p,'%s_%s_y'%(rl,m)).compliant = True raw_input('Ready to make hips/abs/chest/arms compliant and put to sit position?') for rl in 'rl': for m in ['ankle', 'knee']: getattr(p,'%s_%s_y'%(rl,m)).compliant = False for m in p.arms: m.compliant = True p.abs_y.compliant = True p.bust_y.compliant = True raw_input('Ready to sit?') with open("sit_angles.pkl","r") as f: sit_angles = pkl.load(f) for m in p.motors: m.compliant = False p.goto_position(sit_angles, 5, wait=True) p.close()
jlims = { "head_y": [-22., 20.], # backward/forward "head_z": [-58., 40.], # right/left "l_ankle_y": [-40, 40], # flex/point "r_ankle_y": [-40, 40], # flex/point } fps = 10 # moving_speed = 25 p = PH() for m in jlims.keys(): getattr(p, m).compliant = False # getattr(p,m).moving_speed = moving_speed p.goto_position({"r_ankle_y": -40, "l_ankle_y": 40}, 1, wait=True) c = OpenCVCamera("poppy-cam", 0, fps) def handle_close(event): c.close() p.close() sys.exit(0) plt.ion() fig = plt.figure() fig.canvas.mpl_connect("close_event", handle_close) while True: