'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:
