def gen_leg(x, y):
ret = []
base = [0, 3, 0]
lengths = [1.5, 3]
angles = [-np.pi, -np.pi/2]
p = forward_kinematics(base, lengths, angles)
for i in range(len(x)):
p = fabrik(p, lengths, np.array([x[i], y[i], 0]), 1e-5)
p[0] = np.array(base)
ret.append(p.copy())
ret_stroke = np.linspace(x.min(), x.max(), int(len(x)/4))
for i in range(len(ret_stroke)):
p = fabrik(p, lengths, np.array([ret_stroke[i], y.max(), 0]), 1e-5)
p[0] = np.array(base)
ret.append(p.copy())
return np.array(ret)
x = b - a
y = c - b
cross = np.cross(x, y)
ang = np.arctan2(np.linalg.norm(cross), np.dot(x, y))
if np.dot(n, cross) < 0:
ang = -ang
return np.degrees(ang)
base = [0, 3, 0]
lengths = [2, 3]
angles = [-np.pi * 0, -np.pi/2]
p = forward_kinematics(base, lengths, angles)
p = fabrik(p, lengths, np.array([x[10], y[10], 0]), 1e-5)
p[0] = np.array(base)
p = np.array(p)
fab = fabrik(p.copy(), lengths, np.array([x[250], y[250], 0]), 1e-5, step=True)
fab = np.array(fab)
def a1():
x = p.T[0]
y = p.T[1]
j1.set_data([x[0], x[1]], [y[0], y[1]])
j2.set_data([x[1], x[2]], [y[1], y[2]])
def a2():
t.set_data(x[250], y[250])
