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])