def position_leg(l, ex, ey, sx, sy, zl=1.2, zr=0.2, dz=0.01, bs=None):
if bs is None:
bs = {}
z = zl
# lift
print("lifting")
while z > zr:
s = bs.copy()
s[l] = leg.compute_angles(sx, sy, z)
yield s
z -= dz
z = zr
# move
dx = (ex - sx) / 50.
dy = (ey - sy) / 50.
x = sx
y = sy
vx = (dx == 0) or abs(x - ex) < abs(dx)
vy = (dy == 0) or abs(y - ey) < abs(dy)
print("moving")
while not vx or not vy:
s = bs.copy()
s[l] = leg.compute_angles(x, y, z)
yield s
if not vx:
x += dx
else:
x = ex
if not vy:
y += dy
else:
y = ey
vx = (dx == 0) or abs(x - ex) < abs(dx)
vy = (dy == 0) or abs(y - ey) < abs(dy)
# lower
print("lowering")
while z < zl:
s = bs.copy()
s[l] = leg.compute_angles(ex, ey, z)
yield s
z += dz
def stand(start_z=0.2, end_z=1.2, dz=0.01, x=0.9, y=0.):
z = start_z
while z < end_z:
s = {}
for l in legs:
px, py, pz = x, y, z
#if l[1] == 'l':
# px, py, pz = unproject_point(l, x, y, z)
#elif l[1] == 'r':
# px, py, pz = unproject_point(l, x, -y, z)
s[l] = leg.compute_angles(px, py, pz)
z += dz
yield s
def stand(z):
for l in legs:
h, t, k = leg.compute_angles(1.1, 0.0, z)
pub(l, 'hip', h)
pub(l, 'thigh', t)
pub(l, 'knee', k)
def stand(z):
for l in legs:
h, t, k = leg.compute_angles(1.1, 0.0, z)
pub(l, 'hip', h)
pub(l, 'thigh', t)
pub(l, 'knee', k)