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