Inverse Kinematics (Pointing Arm) Problem

Hey guys! I hope you all do very very well! Being direct, I was testing a script of inverse kinematics, but I got I problem, and It’s that when the block goes way too far from the character’s body, it’s arm goes away with it:

-Here’s the video:

I don’t really know how to fix this, and I’ll be very very VERY grateful with you and your heart if you can give me a hand with this, I don’t really know how to fix this, thanks for reading and have a nice day! :wink:

My goal is to make the arm move with inverse kinematics to where it’s this red block but without quitting the character’s shoulder position.

Something like this:

image

-Here’s the script (ServerScript):

local pi				=math.pi
local halfpi			=pi/2
local forwardV3			=Vector3.new(0,0,-1) -- Constant

--[[ 
	
		OriginCFrame is the CF of the shoulder joint, this can be gotten using some CFrame:
		upperTorso.CFrame * shoulderJoint.C0
		Note that the shoulderJoint.C0 would be the INITIAL shoulderJoint C0, before anything has been done
		
		targetPos is the target Position you are trying to point to, this will be a Vector3 in the world space
		
		a is the length of the upper arm, I use 0.515
		
		b is the length of the lower arm + the hand, I use 1.1031
--]]

local function solveArm(originCFrame,targetPosition,a,b)
	-- Localize the targetPosition in regards to originCFrame
	local localized = originCFrame:pointToObjectSpace(targetPosition)
	local localizedUnit = localized.unit
	
	-- Construct a CFrame pointing from the shoulder position to the target position
	local axis = forwardV3:Cross(localizedUnit)
	local angle = math.acos(-localizedUnit.Z)
	local plane = originCFrame*CFrame.fromAxisAngle(axis,angle)
	
	-- Get the length from the origin to our target position
	local c = localized.Magnitude
	
	-- If c is between the lengths of a and b then return an offsetted plane so that one of the lengths reaches the goal,
	-- but the other length is folded so it looks natural
	if c < math.max(a,b)-math.min(a,b) then
		return plane * CFrame.new(0,0,math.max(b,a)-math.min(b-a)-c), -halfpi, pi
		
	-- If c > a + b then return flat angles and an offsetted plane which reaches its target
	elseif c > a+b then
		return plane * CFrame.new(0,0,a+b-c), halfpi, 0
		
	-- Otherwise, use the law of cosines
	else
		local theta1 = -math.acos((-(b * b) + (a * a) + (c * c)) / (2 * a * c)) -- law of cosines, gets the angle opposite b
		local theta2 = math.acos(((b  * b) - (a * a) + (c * c)) / (2 * b * c)) -- law of cosines, gets the angle opposite a
		return plane, theta1 + halfpi, theta2 - theta1
	end
end
	
-- Demonstration for how this can be called:

local targetObj			=workspace:WaitForChild("Target")

local character			=workspace:WaitForChild("Test")
local upperTorso		=character:WaitForChild("UpperTorso")
local rightUpperArm		=character:WaitForChild("RightUpperArm")
local rightLowerArm		=character:WaitForChild("RightLowerArm")
local rightShoulder		=rightUpperArm:WaitForChild("RightShoulder")
local rightElbow		=rightLowerArm:WaitForChild("RightElbow")
local rightShoulderInit	=rightShoulder.C0	-- This can be a preset value, since it will always be the same, it is the offset from the UpperTorso to the RightShoulder joint
local rightElbowInit	=rightElbow.C0		-- This can be a preset value, since it will always be the same, it is the offset from the RightUpperArm to the RightElbow joint

local tp0				=CFrame.new((upperTorso.CFrame * CFrame.new(1.25,0,-1)).p)*CFrame.Angles(0,math.pi/2,0)

while wait() do
	local targetPos		=targetObj.Position
	local shoulderCF	=upperTorso.CFrame * rightShoulderInit
	local plane, shoulderAngle, elbowAngle	=solveArm(shoulderCF, targetPos, 0.515, 1.031)
	rightShoulder.C0 	=upperTorso.CFrame:toObjectSpace(plane) * CFrame.Angles(shoulderAngle, 0, 0)
	rightElbow.C0		=rightElbowInit * CFrame.Angles(elbowAngle, 0, 0)
end

Even if you can give me the smallest hand, I’ll be grateful with it, thank you and byee! :wink: :grinning_face_with_smiling_eyes: :wave:

1 Like

Since the red block should only be limited to a sphere radius from the shoulder joint you should clamp c's length to that magnitude.
math.clamp is explained here better than I could do it:

1 Like

The rotation of the right shoulder seems correct, it’s just the position given by the solver.

Consequently, just maintain the Rotation of the CFrame and keep the original position vector.

local goalRightShoulderC0CFrame = upperTorso.CFrame:toObjectSpace(plane) * CFrame.Angles(shoulderAngle, 0, 0)
	rightShoulder.C0 	=goalRightShoulderC0CFrame.Rotation + rightShoulder.C0.Position
	rightElbow.C0		=rightElbowInit * CFrame.Angles(elbowAngle, 0, 0)
2 Likes

Omg, really really thanks for your help! I don’t know how to thank you, really really thanks for helping me with this problem! You made my day :grinning_face_with_smiling_eyes: :wink: :wave:

Also have a nice day! And don’t forget to smile!

I know this has already been solved, but I figured I’d chime in. When I originally wrote this code, I was using it for a weapon system and wanted it to reach the target position no matter what.

This scenario is actually one of the cases the solver checks for: c > a+b. In this case the solver knows that the target offset is greater than the combined length of a and b.

The existing code does this:

return plane * CFrame.new(0,0,a+b-c), halfpi, 0

notice how it first moves it backwards by the total arm length with a + b, and then moves it forward by the target offset length with -c.

In your case you would just need it to not move it forwards at all. This is actually a very simple resulting calculation:

return plane, halfpi, 0

The final solver code would then just be:

local function solveArm(originCFrame,targetPosition,a,b)
	-- Localize the targetPosition in regards to originCFrame
	local localized = originCFrame:pointToObjectSpace(targetPosition)
	local localizedUnit = localized.unit
	
	-- Construct a CFrame pointing from the shoulder position to the target position
	local axis = forwardV3:Cross(localizedUnit)
	local angle = math.acos(-localizedUnit.Z)
	local plane = originCFrame*CFrame.fromAxisAngle(axis,angle)
	
	-- Get the length from the origin to our target position
	local c = localized.Magnitude
	
	-- If c is between the lengths of a and b then return an offsetted plane so that one of the lengths reaches the goal,
	-- but the other length is folded so it looks natural
	if c < math.max(a,b)-math.min(a,b) then
		return plane * CFrame.new(0,0,math.max(b,a)-math.min(b-a)-c), -halfpi, pi
		
	-- If c > a + b then return flat angles and an offsetted plane which reaches its target
	elseif c > a+b then
		return plane, halfpi, 0
		
	-- Otherwise, use the law of cosines
	else
		local theta1 = -math.acos((-(b * b) + (a * a) + (c * c)) / (2 * a * c)) -- law of cosines, gets the angle opposite b
		local theta2 = math.acos(((b  * b) - (a * a) + (c * c)) / (2 * b * c)) -- law of cosines, gets the angle opposite a
		return plane, theta1 + halfpi, theta2 - theta1
	end
end

That should be a very efficient solution to this problem. I do think the overall inverse kinematics code could be improved (just critiquing my old code), but at least for this problem it is a very efficient solution.

4 Likes