FABRIK with pole

GuinPeng · October 13, 2024, 12:53pm

As the name suggests, I have a working FABRIK, although I have no idea where to start on the pole; to give you a hindsight, I want the elbow of a limb to rotate on a so called “Imaginary plane” to get closer to the pole. (Or if you have any other methods, that’s fine too.)

the code I have for FABRIK just incase it’s needed :

function IK.FABRIK(Limb)
	
	-- Backwards
	Limb["Joint"..tostring(Limb["Number of joints"]) - 1].Position =
		Limb["Target"].Position
	for i = Limb["Number of joints"] - 2, 0, -1 do
		Reach(Limb["Joint"..tostring(i)],
			Limb["Joint"..tostring(i + 1)],
			Limb["Segment"..tostring(i)]
		)
	end
	
	
	-- Forwards
	Limb["Joint0"].Position = Limb["Joint0"]:GetAttribute("Origin")
	for i = 1, Limb["Number of joints"] - 1 do
		Reach(Limb["Joint"..tostring(i)],
			Limb["Joint"..tostring(i - 1)],
			Limb["Segment"..tostring(i - 1)]
		)
	end
	
end

function Reach(CurrentJoint, EndJoint, Length)
	local Distance = (EndJoint.Position 
		- CurrentJoint.Position).Magnitude
	CurrentJoint.CFrame = CFrame.lookAt(CurrentJoint.Position,
		EndJoint.Position
	)
	CurrentJoint.CFrame *= CFrame.new(-Vector3.zAxis * (Distance -
		Length))
end

(Pls, I’ve been trying to work this out for months )

GuinPeng · October 14, 2024, 3:53am

Boost
Have made no progress so far

GuinPeng · October 23, 2024, 2:36am

Boost again :((
abcdefghijklmnopqrstuvwxyz

SLPM · October 23, 2024, 3:42am

Create a Plane
Calculate Pole Influence
Apply Rotation

Adjusted Code with Pole Vector Integration:

function IK.FABRIK(Limb, PolePosition)
	
	-- Backwards pass: Set end effector position first and move towards the root
	Limb["Joint"..tostring(Limb["Number of joints"] - 1)].Position = Limb["Target"].Position
	for i = Limb["Number of joints"] - 2, 0, -1 do
		Reach(Limb["Joint"..tostring(i)], Limb["Joint"..tostring(i + 1)], Limb["Segment"..tostring(i)])
	end
	
	-- Apply Pole Vector Constraint on the middle joint (elbow or knee)
	ApplyPoleVectorConstraint(Limb, PolePosition)
	
	-- Forwards pass: Move from the root towards the end effector
	Limb["Joint0"].Position = Limb["Joint0"]:GetAttribute("Origin")
	for i = 1, Limb["Number of joints"] - 1 do
		Reach(Limb["Joint"..tostring(i)], Limb["Joint"..tostring(i - 1)], Limb["Segment"..tostring(i - 1)])
	end
end

function Reach(CurrentJoint, EndJoint, Length)
	local Distance = (EndJoint.Position - CurrentJoint.Position).Magnitude
	CurrentJoint.CFrame = CFrame.lookAt(CurrentJoint.Position, EndJoint.Position)
	CurrentJoint.CFrame *= CFrame.new(-Vector3.zAxis * (Distance - Length))
end

-- Function to apply pole vector influence on the middle joint (elbow or knee)
function ApplyPoleVectorConstraint(Limb, PolePosition)
	local rootJoint = Limb["Joint0"]
	local endEffector = Limb["Joint"..tostring(Limb["Number of joints"] - 1)]
	local middleJoint = Limb["Joint"..tostring(math.floor(Limb["Number of joints"] / 2))]

	-- Calculate vectors from the root to the middle joint and the end effector
	local rootToMiddle = middleJoint.Position - rootJoint.Position
	local rootToEndEffector = endEffector.Position - rootJoint.Position
	
	-- Project the middle joint onto the plane defined by the root, end effector, and pole
	local planeNormal = rootToEndEffector:Cross(PolePosition - rootJoint.Position).Unit
	local projectedMiddle = middleJoint.Position - (rootToMiddle:Dot(planeNormal) * planeNormal)

	-- Adjust the middle joint's position to align with the pole vector
	middleJoint.Position = projectedMiddle
end

This calculates the vector from the root to the middle joint and the root to the end effector. It then projects the middle joint onto a plane formed by the root and end effector, influenced by the pole vector. This ensures the middle joint (e.g., elbow or knee) bends in a way that aligns with the pole.

GuinPeng · October 23, 2024, 3:50am

This doesn’t change anything at all. It has no errors; yet the Limb is still acting like as if it has no pole.

SLPM · October 23, 2024, 3:53am

try this

function ApplyPoleVectorConstraint(Limb, PolePosition)
    local rootJoint = Limb["Joint0"]
    local endEffector = Limb["Joint"..tostring(Limb["Number of joints"] - 1)]
    local middleJoint = Limb["Joint"..tostring(math.floor(Limb["Number of joints"] / 2))]

    local rootToEndEffector = (endEffector.Position - rootJoint.Position).Unit
    local rootToPole = (PolePosition - rootJoint.Position).Unit

    -- Calculate the normal to the plane formed by the root, end effector, and pole
    local planeNormal = rootToEndEffector:Cross(rootToPole).Unit
    
    -- Calculate the projection of the middle joint onto this plane
    local projectedMiddle = rootToEndEffector:Cross(planeNormal).Unit

    -- Adjust middle joint position to align with the pole vector
    middleJoint.Position = rootJoint.Position + projectedMiddle * (middleJoint.Position - rootJoint.Position).Magnitude
end

GuinPeng · October 23, 2024, 3:56am

It made my Inverse kinematics worse somehow, it completely breaks it

SLPM · October 23, 2024, 4:02am

Sorry about that going to try another approach…

SLPM · October 23, 2024, 4:03am

local function solveIKWithPole<T>(Limb: T, PolePosition: Vector3, numJoints: number)
    -- Generic IK solver for any limb, accepting number of joints and pole vector
    local rootJoint = Limb["Joint0"]
    local endEffector = Limb["Joint" .. tostring(numJoints - 1)]
    local middleJoint = Limb["Joint" .. tostring(math.floor(numJoints / 2))]
    local rootToEndEffector = (endEffector.Position - rootJoint.Position).Unit
    local rootToPole = (PolePosition - rootJoint.Position).Unit
    local planeNormal = rootToEndEffector:Cross(rootToPole).Unit
    local toMiddleJoint = middleJoint.Position - rootJoint.Position
    local projectedMiddleJoint = toMiddleJoint - (toMiddleJoint:Dot(planeNormal) * planeNormal)
    middleJoint.Position = rootJoint.Position + projectedMiddleJoint
end

-- FABRIK solver
function IK.FABRIK<T>(Limb: T, PolePosition: Vector3)
    -- Generic function to apply the pole vector constraint and perform FABRIK
    local numJoints = Limb["Number of joints"]
    
    -- Backward pass: Move from end-effector to root
    Limb["Joint" .. tostring(numJoints - 1)].Position = Limb["Target"].Position
    for i = numJoints - 2, 0, -1 do
        Reach(Limb["Joint" .. tostring(i)], Limb["Joint" .. tostring(i + 1)], Limb["Segment" .. tostring(i)])
    end

    -- Apply the pole vector constraint using the generic solver
    solveIKWithPole(Limb, PolePosition, numJoints)

    -- Forward pass: Move from root to end-effector
    Limb["Joint0"].Position = Limb["Joint0"]:GetAttribute("Origin")
    for i = 1, numJoints - 1 do
        Reach(Limb["Joint" .. tostring(i)], Limb["Joint" .. tostring(i - 1)], Limb["Segment" .. tostring(i - 1)])
    end
end

function Reach(CurrentJoint, EndJoint, Length)
    local Distance = (EndJoint.Position - CurrentJoint.Position).Magnitude
    CurrentJoint.CFrame = CFrame.lookAt(CurrentJoint.Position, EndJoint.Position)
    CurrentJoint.CFrame *= CFrame.new(-Vector3.zAxis * (Distance - Length))
end

GuinPeng · October 23, 2024, 4:07am

Same result… No errors, but the limb still bends uncorrectly as if it doesn’t have a pole

SLPM · October 23, 2024, 4:14am

function ApplyPoleVectorConstraint(Limb, PolePosition)
    local rootJoint = Limb["Joint0"]
    local endEffector = Limb["Joint"..tostring(Limb["Number of joints"] - 1)]
    local middleJoint = Limb["Joint"..tostring(math.floor(Limb["Number of joints"] / 2))]
    local rootToEnd = (endEffector.Position - rootJoint.Position).Unit
    local rootToPole = (PolePosition - rootJoint.Position).Unit
    local planeNormal = rootToEnd:Cross(rootToPole).Unit
    local middleDistance = (middleJoint.Position - rootJoint.Position).Magnitude
    local newMiddlePos = rootJoint.Position + planeNormal * middleDistance

    middleJoint.Position = newMiddlePos
end

function IK.FABRIK(Limb, PolePosition)
    local numJoints = Limb["Number of joints"]
    
    -- Backward pass
    Limb["Joint"..tostring(numJoints - 1)].Position = Limb["Target"].Position
    for i = numJoints - 2, 0, -1 do
        Reach(Limb["Joint"..tostring(i)], Limb["Joint"..tostring(i + 1)], Limb["Segment"..tostring(i)])
    end

    -- Apply the pole vector constraint 
    ApplyPoleVectorConstraint(Limb, PolePosition)

    -- Forward pass
    Limb["Joint0"].Position = Limb["Joint0"]:GetAttribute("Origin")
    for i = 1, numJoints - 1 do
        Reach(Limb["Joint"..tostring(i)], Limb["Joint"..tostring(i - 1)], Limb["Segment"..tostring(i - 1)])
    end
end

-- Limb Chain
function Reach(CurrentJoint, EndJoint, Length)
    local Distance = (EndJoint.Position - CurrentJoint.Position).Magnitude
    CurrentJoint.CFrame = CFrame.lookAt(CurrentJoint.Position, EndJoint.Position)
    CurrentJoint.CFrame *= CFrame.new(-Vector3.zAxis * (Distance - Length))
end

If this one doesn’t work can you send me copy of your setup so I can try to test?

GuinPeng · October 23, 2024, 4:21am

It made the fabrik break

Here’s the game file:
FABRIKwithNoPole.rbxl (57.0 KB)
I reverted it back to the start where there’s no pole function, just be sure to run it and not play test.

SLPM · October 23, 2024, 4:35am

This is what I came up with for your Module. I think I did what you wanted, but let me know:

local IK = {}

function IK.Startup(LimbParent)
	local Limb = {}
	local JointCounter = 0
	local PoleCounter = 0
	local Length = 0

	for _, Part : Part in pairs(LimbParent:GetChildren()) do
		if Part:GetAttribute("Joint") then
			Limb["Joint"..tostring(Part:GetAttribute("Joint"))] = Part
			JointCounter += 1
		elseif Part:GetAttribute("Target") then
			Limb["Target"] = Part
		elseif Part:GetAttribute("Pole") then
			Limb["Pole"..tostring(Part:GetAttribute("Pole"))] = Part
			PoleCounter += 1
		end
	end

	Limb["Number of joints"] = JointCounter

	for i = 0, JointCounter - 2, 1 do
		Limb["Segment"..tostring(i)] =
			(Limb["Joint"..tostring(i)].Position -
				Limb["Joint"..tostring(i + 1)].Position).Magnitude
		Length += Limb["Segment"..tostring(i)]
	end

	Limb["Length"] = Length

	return Limb
end

-- Function to apply pole vector influence on the middle joint (elbow) with damping
function ApplyPoleVectorConstraint(Limb)
	local rootJoint = Limb["Joint0"]
	local middleJoint = Limb["Joint"..tostring(math.floor(Limb["Number of joints"] / 2))]
	local endEffector = Limb["Joint"..tostring(Limb["Number of joints"] - 1)]
	local pole = Limb["Pole1"].Position  -- Assuming there's one pole in your setup

	-- Vector from root to end-effector
	local rootToEnd = (endEffector.Position - rootJoint.Position).Unit

	-- Vector from root to pole
	local rootToPole = (pole - rootJoint.Position).Unit

	-- Vector from root to middle joint
	local rootToMiddle = (middleJoint.Position - rootJoint.Position).Unit

	-- Plane normal formed by root to end and root to pole
	local planeNormal = rootToEnd:Cross(rootToPole).Unit

	-- Calculate the projection of the middle joint direction onto the plane normal
	local projectedMiddle = (rootToMiddle - planeNormal * rootToMiddle:Dot(planeNormal)).Unit

	-- Calculate rotation direction and angle to align middle joint with pole
	local rotationAxis = rootToEnd
	local dotProduct = projectedMiddle:Dot(rootToPole)

	-- Clamp the dot product to avoid numerical instability
	dotProduct = math.clamp(dotProduct, -1, 1)

	-- Calculate the angle based on the clamped dot product
	local rotationAngle = math.acos(dotProduct)

	-- Apply smoothing using lerp (damping)
	local dampingFactor = 0.1  -- Adjust this value for stronger or weaker damping
	if math.abs(rotationAngle) > 0.001 then
		-- Interpolate between the current CFrame and the target CFrame
		local targetCFrame = rootJoint.CFrame * CFrame.fromAxisAngle(rotationAxis, rotationAngle) * CFrame.new(0, 0, -(middleJoint.Position - rootJoint.Position).Magnitude)
		middleJoint.CFrame = middleJoint.CFrame:Lerp(targetCFrame, dampingFactor)
	end

	-- DEBUG: Visualize vectors for further tracking
	print("Pole:", pole, "MiddleJoint:", middleJoint.Position, "Root:", rootJoint.Position)
	print("rootToEnd:", rootToEnd, "rootToPole:", rootToPole, "planeNormal:", planeNormal, "rotationAngle:", math.deg(rotationAngle))
end

function IK.FABRIK(Limb)
	-- Backwards pass: move from end-effector to root
	Limb["Joint"..tostring(Limb["Number of joints"] - 1)].Position = Limb["Target"].Position
	for i = Limb["Number of joints"] - 2, 0, -1 do
		Reach(Limb["Joint"..tostring(i)], Limb["Joint"..tostring(i + 1)], Limb["Segment"..tostring(i)])
	end

	-- Apply Pole Constraint to the middle joint after backward pass
	ApplyPoleVectorConstraint(Limb)

	-- Forwards pass: move from root to end-effector
	Limb["Joint0"].Position = Limb["Joint0"]:GetAttribute("Origin")
	for i = 1, Limb["Number of joints"] - 1 do
		Reach(Limb["Joint"..tostring(i)], Limb["Joint"..tostring(i - 1)], Limb["Segment"..tostring(i - 1)])
	end
end

-- Reach function remains unchanged
function Reach(CurrentJoint, EndJoint, Length)
	local Distance = (EndJoint.Position - CurrentJoint.Position).Magnitude
	CurrentJoint.CFrame = CFrame.lookAt(CurrentJoint.Position, EndJoint.Position)
	CurrentJoint.CFrame *= CFrame.new(-Vector3.zAxis * (Distance - Length))
end

return IK

GuinPeng · October 23, 2024, 4:45am

I feel like we’re on completely different pages here, what I’m trying to achieve is this;

Where the red highlight in game is the lightest green in the video above, and the yellow highlight in the game is the Pole aka the cylinder you can see in the video that’s attracting the elbow to stop it from bending the other way.

SLPM · October 23, 2024, 4:55am

Maybe I’m confused but I feel like what we got looks kind of like that already

GuinPeng · October 23, 2024, 5:02am

This in itself looks wrong ++ you can look at the time stamp 0:04 and see how it reacts falsely

SLPM · October 23, 2024, 5:25am

I made an improvement to stabilize it more

-- Function to apply pole vector influence on the middle joint (elbow) with damping and velocity stabilization
function ApplyPoleVectorConstraint(Limb)
	local rootJoint = Limb["Joint0"]
	local middleJoint = Limb["Joint"..tostring(math.floor(Limb["Number of joints"] / 2))]
	local endEffector = Limb["Joint"..tostring(Limb["Number of joints"] - 1)]
	local pole = Limb["Pole1"].Position  -- Assuming there's one pole in your setup

	-- Vector from root to end-effector
	local rootToEnd = (endEffector.Position - rootJoint.Position).Unit

	-- Vector from root to pole
	local rootToPole = (pole - rootJoint.Position).Unit

	-- Vector from root to middle joint
	local rootToMiddle = (middleJoint.Position - rootJoint.Position).Unit

	-- Plane normal formed by root to end and root to pole
	local planeNormal = rootToEnd:Cross(rootToPole).Unit

	-- Calculate the projection of the middle joint direction onto the plane normal
	local projectedMiddle = (rootToMiddle - planeNormal * rootToMiddle:Dot(planeNormal)).Unit

	-- Calculate rotation direction and angle to align middle joint with pole
	local rotationAxis = rootToEnd
	local dotProduct = projectedMiddle:Dot(rootToPole)

	-- Clamp the dot product to avoid numerical instability
	dotProduct = math.clamp(dotProduct, -1, 1)

	-- Calculate the angle based on the clamped dot product
	local rotationAngle = math.acos(dotProduct)

	-- Apply stronger stabilization to avoid overcorrections
	local settleThreshold = 0.1  -- Increase the threshold to make it less sensitive to small errors
	local dampingFactor = 0.02    -- Lower damping for smoother transitions

	-- If the rotation angle is greater than the threshold, apply the rotation with damping
	if math.abs(rotationAngle) > settleThreshold then
		-- Interpolate between the current CFrame and the target CFrame for smoother movement
		local targetCFrame = rootJoint.CFrame * CFrame.fromAxisAngle(rotationAxis, rotationAngle) * CFrame.new(0, 0, -(middleJoint.Position - rootJoint.Position).Magnitude)

		-- Smoother transition using a more aggressive lerp factor
		middleJoint.CFrame = middleJoint.CFrame:Lerp(targetCFrame, dampingFactor)
	else
		-- Consider the joint settled if it's within the threshold
		print("Elbow settled near the pole.")
	end

	-- DEBUG: Print out for further tracking
	print("Pole:", pole, "MiddleJoint:", middleJoint.Position, "Root:", rootJoint.Position)
	print("rootToEnd:", rootToEnd, "rootToPole:", rootToPole, "planeNormal:", planeNormal, "rotationAngle:", math.deg(rotationAngle))
end

I feel like it’s not quite where it needs to be but it’s pretty close. Going to try to refine it more. It is definitely positioning based on the Pole

GuinPeng · October 23, 2024, 5:35am

It looks like you just made it slower

SLPM · October 23, 2024, 5:38am

It doesn’t do the spin around thing in as many angles.

SLPM · October 23, 2024, 6:06am

Think I am getting closer to the intended outcome but it’s still not right…

function ApplyPoleVectorConstraint(Limb)
	local rootJoint = Limb["Joint0"]
	local middleJoint = Limb["Joint"..tostring(math.floor(Limb["Number of joints"] / 2))]
	local endEffector = Limb["Joint"..tostring(Limb["Number of joints"] - 1)]
	local pole = Limb["Pole1"].Position

	-- Vector from root to end-effector
	local rootToEnd = (endEffector.Position - rootJoint.Position).Unit

	-- Vector from root to pole
	local rootToPole = (pole - rootJoint.Position).Unit

	-- Vector from root to middle joint
	local rootToMiddle = middleJoint.Position - rootJoint.Position

	-- Plane normal formed by root-to-end and root-to-pole
	local planeNormal = rootToEnd:Cross(rootToPole).Unit

	-- Project the middle joint onto the plane normal to enforce the constraint
	local projectionOntoPlane = rootToMiddle - planeNormal * rootToMiddle:Dot(planeNormal)

	-- Ensure that the middle joint stays on the side of the pole (prevent crossing the plane)
	local dotProduct = rootToPole:Dot(projectionOntoPlane.Unit)

	-- Flip the projection if it crosses the pole
	if dotProduct < 0 then
		projectionOntoPlane = projectionOntoPlane * -1
		print("Elbow flipped back to the correct side of the pole.")
	end

	-- Move the middle joint to the corrected position on the plane
	middleJoint.Position = rootJoint.Position + projectionOntoPlane

	-- Add a stabilization factor to avoid jitter
	local dampingFactor = 0.2
	middleJoint.CFrame = middleJoint.CFrame:Lerp(CFrame.new(middleJoint.Position), dampingFactor)

	-- Debugging info
	print("Pole:", pole, "MiddleJoint:", middleJoint.Position, "Root:", rootJoint.Position)
	print("rootToEnd:", rootToEnd, "rootToPole:", rootToPole, "planeNormal:", planeNormal)
end