def _fabrik_reach(p, d, start=None, unreachable=False, target=None):
"""Direction independent implementation of the "Reach" steps in FABRIK.
- p: a list of joints;
- d: for each d[i] it is the distance between p[i] and p[i + 1];
- start: the start location of p[0];
- unreachable: whether the target is reachable;
- target: the target to reach if unreachable.
It returns a list of new joint locations and orientations.
"""
if start is None:
start = p[0]
new_p = [start]
if unreachable:
p = len(p) * [target]
for next_p, curr_d in zip(p[1,:], d):
curr_p = new_p[-1]
r = distance(next_p, curr_p)
labda = curr_d / r
# Find the new joint locationitions
if len(new_p) > 1:
prev_p = new_p[-2]
else:
prev_p = new_p[-1]
new_p.append(_update(labda, prev_p, curr_p, next_p))
return new_p
def fabrik(p, t, d):
"""Forward And Backward Reaching Inverse Kinematics solver.
For a discussion of this algorithm, look for an article or technical
report by Aristidou and Lasenby. I used "CUED/F-INFENG/TR-632".
"""
# Check whether the target is within reach
dist = distance(p[0] - t)
if dist > sum(d):
p = _fabrik_reach(p, d, unreachable=True, target=t)
else:
b = p[0]
# If the end effector is close enough to the target, finish.
while distance(p[-1], t) > TOLERANCE:
# FORWARD REACHING
p = _fabrik_reach(reversed(p), reversed(d), t)
# STAGE 2: BACKWARD REACHIN
p = _fabrik_reach(p, d, b)
return p
