Joint rotation to replicate a position change.

Hi all,

So among other things I’ve been working on IK stuff, but I’ve wasted a lot of hours trying to get joints to rotate properly between positions, and since stubborn persistence isn’t working, I figured I’d better ask for some help. The code below is a bit of test code I banged together which should hopefully give enough of an idea of what I’m trying to do. If not I can package up the various bits I have if some needs to play with the code directly.

The short of it is that the solver spits out new positions for joints, and I’m trying to rotate the joint so that it matches this position. Translating the joints works perfectly, apart from the obvious mesh distortion, so I know the solver is working fine. I’ve also tried using lookAt(), but that seems to behave very erratically, and has what looks like gimbal lock type problems.

Here’s a short clip of how it behaves with the below code, and here’s the “correct” behaviour by simply translating the joint.

The Chain class is basically a list of NodePaths representing joints, where the root of the chain is an exposeJoint, updated each frame with the net transform of the joint it represents, and each descendant of that NodePath is a controlJoint representing the child joints.

actor = Actor("models/artist2.egg")

t = loader.loadModel("misc/sphere.egg.pz")
t.setScale(0.05, 0.05, 0.05)
t.setPos(-0.5, -0.35, 1)

hips = actor.getJoints(jointName="hips")[0]
spine = actor.getJoints(jointName="spine")[0]
spine1 = actor.getJoints(jointName="spine-1")[0]
chest = actor.getJoints(jointName="chest")[0]
chest1 = actor.getJoints(jointName="chest-1")[0]
clavicleR = actor.getJoints(jointName="clavicle.R")[0]
deltoidR = actor.getJoints(jointName="deltoid.R")[0]
upperArmR = actor.getJoints(jointName="upper_arm.R")[0]
forearmR = actor.getJoints(jointName="forearm.R")[0]
handR = actor.getJoints(jointName="hand.R")[0]

#chain = KinematicChain(actor, [clavicleR, deltoidR, upperArmR, forearmR, handR])
chain = KinematicChain(actor, [hips, spine, spine1, chest, chest1, clavicleR, deltoidR, upperArmR, forearmR, handR])
chain[-1].effector = Effector(chain[-1])
chain[-1] = t
solver = FABRIKSolver(chain)

# Get a list of all the joint local positions in bind pose, and then get a normalized vector of these.
oVecs = []
for jc in chain:
  oPos = jc.nodePath.getPos()
  oVec = oPos - oPos.origin()

def solve():
  # An iteration of the solver (forwards and then backwards) gives us new joint positions. These are relative to render.
  newPs = solver.solveBackward(False)

  # Chain is just a list of JointControllers, which are really just controlJoint NodePaths plus some helper code.
  for jc, newP, oVec in zip(chain, newPs, oVecs):
    oPos = jc.nodePath.getPos()

    # Set the joint position to the new global position from the solver.
    chain.setNetPosition(jc.nodePath, newP)

    # Get the new local position of the joint, and get a normalized vector representing it.
    nPos = jc.nodePath.getPos()
    nVec = nPos - nPos.origin()

    # Get the cross product between the old and new local position vectors, so we get an axis of rotation between them as a normalized vector.
    axis = oVec.cross(nVec)

    # Find the angle between the old and new local position vectors.
    angle = oVec.angleDeg(nVec)

    # Move the joint back to its bind pose position.

    if axis.length() > 0:
      # Get a quaternion representing the rotation from the local joint position in bind pose, to the new local joint position from the solver.
      rot = LQuaternionf(0, 0, 0, 0)
      rot.setFromAxisAngle(angle, axis)

      # Set the joint rotation to this quaternion.

This has been driving me mad, so I’d be grateful for any suggestions.

Edit: Problem finally solved with a little help from Chris Rayner, who shared some source code from his implementation. Seems I’d gotten completely the wrong mental picture of coordinate spaces etc. All cleared up now.