Solve positions for nested IK joint system

3jnt_nested_ik.py

import pymel.core as pm
from mgear.core.transform import getChainTransform2

# See https://twitter.com/morganloomis_/status/1307263399210840065?s=20

# Pick joint chains
jnts = [pm.PyNode("jnt" + str(i)) for i in range(4)]
helperJnts = [pm.PyNode("helperJnt" + str(i)) for i in range(3)]

# Read world space xforms and positions
jntXform = [j.getMatrix(ws=1) for j in jnts]
Jn = [j.translate for j in jntXform]
L = sum([(Jn[i + 1] - Jn[i]).length() for i in range(3)])

# Compute ja and jn 
ja = (Jn[1] - Jn[0])
ja.normalize()
Jfull = Jn[3] - Jn[0]


A = (Jfull * Jfull - L * L) / (2 * (ja * Jfull - L))


hj1_pos = [Jn[0], Jn[0] + ja * A, Jn[3]]
hj1_norm = pm.dt.cross(Jn[1] - Jn[0], Jn[2] - Jn[1])
hj_xform = getChainTransform2(hj1_pos, hj1_norm)


helperJnts[0].setMatrix(hj_xform[0], ws=1)
helperJnts[1].setMatrix(hj_xform[1], ws=1)
helperJnts[2].setMatrix(hj_xform[2], ws=1)