class FullIK(object):
def __init__(self, platform_radius, neck_length, base_radius, lower_leg_length, upper_leg_length):
self.platform_ik = PlatformIK(platform_radius, neck_length)
self.base_radius = base_radius
self.lower_leg_length = lower_leg_length
self.upper_leg_length = upper_leg_length
def solve(self, e_v, f_v, h):
head_angle, centroid, points = self.platform_ik.solve(e_v, f_v, h)
angles = []
for i in range(3):
platform_point = points[:, i]
angle = np.radians(120.0) * i
rot_z = np.array([[np.cos(angle), -np.sin(angle), 0], [np.sin(angle), np.cos(angle), 0], [0, 0, 1]])
base_point = np.dot(rot_z, np.array([self.base_radius, 0, 0]))
angle = LegIK(base_point, platform_point, self.lower_leg_length, self.upper_leg_length).solve()
angles.append(angle)
angles.append(head_angle)
return angles
def __init__(self, platform_radius, neck_length, base_radius, lower_leg_length, upper_leg_length):
self.platform_ik = PlatformIK(platform_radius, neck_length)
self.base_radius = base_radius
self.lower_leg_length = lower_leg_length
self.upper_leg_length = upper_leg_length
