def recalc_from_foot(self):
self.back_ankle = utils.raytrace_line(self.THIGH_SHIN_CHAIN.joints[2], self._thigh_angle, -BACK_ANKLE_LENGTH)
self.back_thigh_connection = utils.raytrace_line(self._thigh_mount,
self._thigh_angle, THIGH_DIST_TO_FOUR_BAR)
self.upper_thigh_connection = utils.raytrace_line(self._thigh_mount, self._thigh_angle, THIGH_DIST_TO_CONNECTOR)
# fabrik(self.upper_thigh_connection, self.RECIP_CHAIN, 30)
self.RECIP_CHAIN.analytic_ik(self.upper_thigh_connection)
def __init__(self):
self.THIGH_SHIN_CHAIN = JointChain(THIGH_MOUNT).add_link(THIGH_LENGTH, 0).add_link(SHIN_LENGTH, 0)
self.RECIP_CHAIN = JointChain(RECIP_MOUNT).add_link(RECIP_LENGTH, 0).add_link(CONNECTOR_LENGTH, 0)
self.ankle_angle = 0
self.back_ankle = utils.raytrace_line(self.THIGH_SHIN_CHAIN.joints[1],
-self.THIGH_SHIN_CHAIN.angles[0], -BACK_ANKLE_LENGTH)
self.back_thigh_connection = utils.raytrace_line(THIGH_MOUNT,
-self.THIGH_SHIN_CHAIN.angles[0], THIGH_DIST_TO_FOUR_BAR)
self.upper_thigh_connection = utils.raytrace_line(THIGH_MOUNT,
self.THIGH_SHIN_CHAIN.angles[0], THIGH_DIST_TO_CONNECTOR)
def add_link(self, link_length, default_angle=0):
self.angles += [math.radians(default_angle)]
self.links += [link_length]
self.joints += [
utils.raytrace_line(self.joints[-1], math.radians(default_angle),
link_length)
]
return self
def legs_ccd(self, goal, iters=30):
for i in range(iters):
self.THIGH_SHIN_CHAIN.angles[1] += math.radians(
utils.angle_between(self.THIGH_SHIN_CHAIN.joints[1], goal) - utils.angle_between(
self.THIGH_SHIN_CHAIN.joints[1], utils.raytrace_line(
self.THIGH_SHIN_CHAIN.joints[2],
self._thigh_angle - math.radians(142.7-180), FRONT_ANKLE_LENGTH)))
self.THIGH_SHIN_CHAIN.angles[1] = utils.bound_between(self.THIGH_SHIN_CHAIN.angles[1],
self._thigh_angle + math.radians(40),
self._thigh_angle + math.radians(140))
self.THIGH_SHIN_CHAIN.recalc()
self._thigh_angle += math.radians(
utils.angle_between(self.THIGH_SHIN_CHAIN.joints[0], goal) - utils.angle_between(
self.THIGH_SHIN_CHAIN.joints[0], utils.raytrace_line(
self.THIGH_SHIN_CHAIN.joints[2],
self._thigh_angle - math.radians(142.7-180), FRONT_ANKLE_LENGTH)))
self._thigh_angle = utils.bound_between(self._thigh_angle, 0.15, 1.45)
self.THIGH_SHIN_CHAIN.recalc()
self.recalc_from_foot()
def recalc(self, recip_angle=None, chain_angle=None):
if recip_angle is None:
recip_angle = self._recip_angle
if chain_angle is None:
chain_angle = self._shin_angle - self._connector_angle - math.pi
self.RECIP_CHAIN.angles[0] = recip_angle
self.RECIP_CHAIN.joints[1] = utils.raytrace_line(self._recip_mount, recip_angle, RECIP_LENGTH)
self.RECIP_CHAIN.angles[1], self._thigh_angle = analytic_2link(
CONNECTOR_LENGTH, THIGH_DIST_TO_CONNECTOR, self.RECIP_CHAIN.joints[1], self._thigh_mount)
self._thigh_angle = -math.pi + self._thigh_angle
if self._thigh_angle > 1.57 or self._thigh_angle < 0:
self.RECIP_CHAIN.angles[1], self._thigh_angle = analytic_2link(
CONNECTOR_LENGTH, THIGH_DIST_TO_CONNECTOR, self.RECIP_CHAIN.joints[1], self._thigh_mount, flip=True)
self._thigh_angle = -math.pi + self._thigh_angle
self._shin_angle = chain_angle + self._connector_angle + math.pi
self.RECIP_CHAIN.recalc()
self.THIGH_SHIN_CHAIN.recalc()
self.back_ankle = utils.raytrace_line(self.THIGH_SHIN_CHAIN.joints[2], self._thigh_angle, -BACK_ANKLE_LENGTH)
self.back_thigh_connection = utils.raytrace_line(self._thigh_mount,
self._thigh_angle, THIGH_DIST_TO_FOUR_BAR)
self.upper_thigh_connection = self.RECIP_CHAIN.joints[2]
def relocate_around_foot(self, angle_around_point=math.pi/2):
reverse_ankle_angle = math.pi - self._ankle_angle
angle_delta = angle_around_point - reverse_ankle_angle
self.THIGH_SHIN_CHAIN.joints[2] = utils.raytrace_line(self._foot_point, -reverse_ankle_angle - angle_delta,
FRONT_ANKLE_LENGTH)
reverse_shin_angle = math.pi - self._shin_angle + angle_delta
self.THIGH_SHIN_CHAIN.joints[1] = utils.raytrace_line(self.THIGH_SHIN_CHAIN.joints[2], -reverse_shin_angle,
SHIN_LENGTH)
reverse_thigh_angle = math.pi - self._thigh_angle + angle_delta
self._thigh_mount = utils.raytrace_line(self.THIGH_SHIN_CHAIN.joints[1], -reverse_thigh_angle,
THIGH_LENGTH)
self._thigh_angle -= angle_delta
self.RECIP_CHAIN.joints[2] = utils.raytrace_line(self._thigh_mount, self._thigh_angle, THIGH_DIST_TO_CONNECTOR)
reverse_connector_angle = math.pi - self._connector_angle + angle_delta
self.RECIP_CHAIN.joints[1] = utils.raytrace_line(self.RECIP_CHAIN.joints[2], -reverse_connector_angle,
CONNECTOR_LENGTH)
reverse_recip_angle = math.pi - self._recip_angle + angle_delta
self._recip_mount = utils.raytrace_line(self.RECIP_CHAIN.joints[1], -reverse_recip_angle, RECIP_LENGTH)
self._connector_angle -= angle_delta
self._recip_angle -= angle_delta
self._shin_angle -= math.pi + angle_delta
self.THIGH_SHIN_CHAIN.recalc()
def recalc(self):
for i, link in enumerate(self.links):
self.joints[i + 1] = utils.raytrace_line(self.joints[i],
self.angles[i], link)
def _foot_point(self):
return utils.raytrace_line(self.THIGH_SHIN_CHAIN.joints[2], self._ankle_angle, FRONT_ANKLE_LENGTH)