 # 2 Joint 2 Limb Inverse Kinematics

Introduction

Some time ago I replied to a thread explaining a method of IK I used and the theory behind it. It’s been linked all over and I figure it could use a more thought out tutorial. This thread is an expanded stand-alone version of that reply. Here you will find a breakdown of a super basic form of IK solving as well an example .rbxl file.

This tutorial assumes you have an understanding of:

• Basic algebra and trigonometry
• CFrames pertaining to welds and operations
• Basic understanding of vectors

Theory
The TL;DR of Inverse Kinematics is that it’s a process of solving a system of joints and limbs to reach from one point to another. There are a number of ways to do this, all varying in complexity and purpose. We’ll be solving a 2 joint 2 limb system by adapting the Law of Cosines. For those unfamiliar the law allows us to take the lengths of a triangle and solve for its angles.

We can put the picture above into the context of an arm by relating the following values:
- Angle A is the angle of the shoulder
- Angle C is the angle of the elbow
- Vertex of angle A is the shoulder position
- Vertex of angle B is the goal position
- Length c is distance from the shoulder to the goal
- Length b is the length of the upper arm
- Length a is the length of the lower arm

Lengths b and a we’ll have to provide, but since they are arm lengths that information should already available to us. Length c is going to be the difference between the shoulder position and the goal position which is something we can easily calculate. We’ll just need to solve for the angles A and C, we can do that by adapting two of the following equations:

The values to solve our system may end up looking something like:

``````b = 1.255
a = 0.735
c = (shoulderPosition - goalPosition).magnitude

A = math.acos((-a^2 + b^2 - c^2) / (2 * b * c))
C = math.acos((a^2 + b^2 - c^2) / (2 * a * b))
``````

Now all that is left to do is apply these values and account for cases in which these values cannot be solved for. This is the stage in which you have some creative decisions to make as to how you want to use your solved values, later on I’ll be showing and explaining the cases used in Apocalypse Rising 2.

Application
When it comes to applying properly solved angles in 3D space we can think of the line making up distance c as a “flat” point of reference between our shoulder and goal position. We can use `CFrame.new(shoulderPosition, goalPosition)` to get a CFrame we can use as that reference point and orientation.

Applying the angles is now just as simple as building out the CFrames. Our shoulder represented as a CFrame is now just `CFrame.Angles(A + math.pi/2, 0, 0)`, and our elbow as a CFrame is `CFrame.Angles(C - A, 0, 0)`.

Depending on your use case you can plug these values in as “offsets” to joint C0 properties, or you may need to use them as references for building out the arm from shoulder to elbow. These may look something like:

``````-- constants used for solving
local shoulderC0Save = shoulder.C0
local elbowC0Save = elbow.C0
local upperArmLength = 1.4
local lowerArmLength = 1.2

-- base values to be calulated at run time
local shoulderCFrame = upperTorso.CFrame * shoulderC0Save
local basePlane = CFrame.new(shoulderCFrame.p, goalPosition)
local shoulderAngle = CFrame.Angles(A + math.pi/2, 0, 0)
local elbowAngle = CFrame.Angles(C - A, 0, 0)

-- weld/motor example
shoulder.C0 = upperTorso.CFrame:ToObjectSpace(basePlane) * shoulderAngle
elbow.C0 = elbowC0Save * elbowAngle

-- building it out example
upperArm.CFrame = basePlane * shoulderAngle * CFrame.new(0, -upperArmLength/2, 0)
lowerArm.CFrame = upperArm.CFrame * CFrame.new(0, -upperArmLength/2, 0) * elbowAngle * CFrame.new(0, -lowerArmLength/2, 0)
``````

Unsolvable Cases
Unsolvable cases occur when the length between the shoulder position and goal position is shorter than the smallest of the arm lengths or is longer than the sum of both arm lengths. The c length in either of these cases will result in the cosine law spitting out numbers that cannot be used.

There are a few ways of solving these situations depending on your use case. In Apocalypse Rising 2 I do the following:

case: point distance is closer than our smallest arm length
solution: push back the shoulder position so the “hand” still reaches, fold the arm into itself

case: point distance is further than our arms can reach
solution: push forward the shoulder position so the “hand” still reaches, arm fully extended

Natural Arm Rotation
This method of IK is useful in cases where you’re trying to model the movement of arms or legs (2 joint 2 limb) in humans. The anatomy of a shoulder doesn’t allow it to rotate in the same ways `CFrame.new(pointA, pointB)` will so we’ll need to come up with a better solution that better mimics the rotation restrictions of a human shoulder.

A simple solution to this problem is to roll our base plane. If you hold your arm out in front of you your elbow is pointed down and your shoulder can be described as having 0 roll. Attempting to mirror that hand position but behind you requires you to roll your shoulder and shift your elbow from pointing downwards to pointing upwards.

Translating that back into code means we’ll have to adjust the information we use to build our plane since we’ll need to factor in shoulder direction to derive the rolling motion. We can do this by changing the `ShoulderPosition` value from a Vector3 to a CFrame since it will better model our base of orientation.

To start we’ll need to find out our goal position localized to our shoulder cframe. This is going to give us a direction vector we can use to base our rolling motion off of.

``````local localized = shoulderCFrame:PointToObjectSpace(goalPosition)
local localizedUnit = localized.unit
``````

We can use cross product and our direction from above to create a new vector that represents the axis we’ll be rolling on. The angle at which we’ll rotate along that axis can be taken from our direction vector as well.

``````local axis = Vector3.new(0, 0, -1):Cross(localizedUnit)
local angle = math.acos(-localizedUnit.Z)
``````

Using our new angle and rotation axis we can replace:

``````local basePlane = CFrame.new(shoulderPosition, goalPosition)
``````

With:

``````local basePlane = originCF * CFrame.fromAxisAngle(axis, angle)
``````

Giving us the same directional behavior as `CFrame.new(pointA, pointB)` but with the more natural rolling motion described above.

Putting Everything Together

So now that we’ve learned how to solve the law of cosines, safeguard against unsolvable cases, and create a better arm rotation motion, we can finally put it all into a single solution. The following code snippet combines everything covered in this thread into a single function:

``````-- originCF: the "Shoulder" CFrame
-- targetPos: the goal position we're trying to reach
-- l1: the length of a segment of arm
-- l2: the length of a segment of arm
local function solveIK(originCF, targetPos, l1, l2)
-- build intial values for solving
local localized = originCF:pointToObjectSpace(targetPos)
local localizedUnit = localized.unit
local l3 = localized.magnitude

-- build a "rolled" planeCF for a more natural arm look
local axis = Vector3.new(0, 0, -1):Cross(localizedUnit)
local angle = math.acos(-localizedUnit.Z)
local planeCF = originCF * CFrame.fromAxisAngle(axis, angle)

-- case: point is to close, unreachable
-- return: push back planeCF so the "hand" still reaches, angles fully compressed
if l3 < math.max(l2, l1) - math.min(l2, l1) then
return planeCF * CFrame.new(0, 0,  math.max(l2, l1) - math.min(l2, l1) - l3), -math.pi/2, math.pi

-- case: point is to far, unreachable
-- return: for forward planeCF so the "hand" still reaches, angles fully extended
elseif l3 > l1 + l2 then
return planeCF * CFrame.new(0, 0, l1 + l2 - l3), math.pi/2, 0

-- case: point is reachable
-- return: planeCF is fine, solve the angles of the triangle
else
local a1 = -math.acos((-(l2 * l2) + (l1 * l1) + (l3 * l3)) / (2 * l1 * l3))
local a2 = math.acos(((l2  * l2) - (l1 * l1) + (l3 * l3)) / (2 * l2 * l3))

return planeCF, a1 + math.pi/2, a2 - a1
end
end
``````

Found below is a more hands on example showcasing usage on systems with no welds/joints and a system that uses character limbs. I’d encourage you all to explore more ways of applying the basic concepts covered here, and I’d love to see what you end up creating.

Accurate play solo may inhibit your ability to interact with the demo file as described. Start server or turning off accurate play solo is recommended to get desired results when testing the jointless solving demo.

197 Likes

This could not have come at a better time. I have literally just started to pick apart the code sample you posted in 2017 and was asking around on the discord earlier.
Crazy how things work some times.

Thanks for this 5 Likes

I found the title amusingly funny; it reminds me of Vin Diesel.

Now that I know this kind of resources, I’ll think about Vin Diesel whenever I hear ‘Limb Inverse Kinematics’.

1 Like

It appears that it is a little bugged.

That’s intended, see the case regarding c length being greater than a + b

I fixed this problem by replacing this

``````return planeCF * cfNew(0, 0, l1 + l2 - l3), pi/2, 0
``````

to this

``````return planeCF, math.pi/2, 0
``````

on the solver

7 Likes

Kind of solves. But when goal is far away it doesn’t point in it’s direction I really like your module and how simple it is, but I’m having so much trouble using it for foot planting. For the most part, it works except its literally backward. I’ve tried adding math.pi to the upper leg angle but it also produces weird results. I’ve been at this for a while now and I’m really lost.

https://i.gyazo.com/5ac959d44c697158c01231be91228d44.mp4

Code: (this is being run every renderstepped) (raycast class is well, just raycast module)

``````local offset = (self.Character.PrimaryPart.CFrame * CFrame.new(0.5, -1, -0)).Position
local hit, position, normal = RaycastClass.CastRayUsingIgnoreList(Ray.new(offset, Vector3.new(0,-5, 0)))

if (offset-position).Magnitude <= 1.9 then
rightHip.Transform = CFrame.new()
rightKnee.Transform = CFrame.new()
rightAnkle.Transform = CFrame.new()

local footCFrame = self.Character.LowerTorso.CFrame * HIP_C0_CACHE
local planeCF, hipAngle, kneeAngle = solveIK(footCFrame, position, UPPER_LENGTH, LOWER_LENGTH)
if lastRightTweens then lastRightTweens:Cancel() end
if lastRightTweens then lastRightTweens:Cancel() end
lastRightTweens = TweenService:Create(rightHip, TweenInfo.new(0.2), {C0 = lowerTorso.CFrame:ToObjectSpace(planeCF) * CFrame.Angles(hipAngle, 0, 0)}):Play()
lastRightTweens = TweenService:Create(rightKnee, TweenInfo.new(0.2), {C0 = KNEE_C0_CACHE * CFrame.Angles(kneeAngle, 0, 0)}):Play()
else
if lastRightTweens then lastRightTweens:Cancel() end
if lastRightTweens then lastRightTweens:Cancel() end
lastRightTweens = TweenService:Create(rightHip, TweenInfo.new(0.5), {C0 = HIP_C0_CACHE}):Play()
lastRightTweens = TweenService:Create(rightKnee, TweenInfo.new(0.5), {C0 = KNEE_C0_CACHE}):Play()
end
--Code being run before:
local rightHip =        char:WaitForChild("RightUpperLeg"):WaitForChild("RightHip")
local rightKnee	= char:WaitForChild("RightLowerLeg"):WaitForChild("RightKnee")
local rightAnkle	= char:WaitForChild("RightFoot"):WaitForChild("RightAnkle")

local solveModule	= ReplicatedStorage.Modules:WaitForChild("SolveIK")
local solveIK		= require(solveModule)

local HIP_C0_CACHE		= rightHip.C0
local KNEE_C0_CACHE		= rightKnee.C0
local ANKLE_C0_CACHE    = rightAnkle.C0

local UPPER_LENGTH			= math.abs(rightHip.C1.Y) + math.abs(rightKnee.C0.Y)
local LOWER_LENGTH			= math.abs(rightKnee.C1.Y) + math.abs(rightAnkle.C0.Y) + math.abs(rightAnkle.C1.Y)
``````

Sorry for replying really late, but I need help with this. Thanks!

try

``````lastRightTweens = TweenService:Create(rightHip, TweenInfo.new(0.2), {C0 = lowerTorso.CFrame:ToObjectSpace(planeCF) * CFrame.Angles(-hipAngle, 0, 0)}):Play()
``````

Not sure if it’d work but I’m guessing by negating the angle, it’d rotate oppositely thereby fixing your problem

It looks right in certain situations but that doesn’t affect how the angle limitations are determined, so once you get to higher limit angles that are either too short or too far then the calculations are reversed so the leg goes short when it should extend the furthest and the leg goes long when it should extend the least.

this is how the IK module looks, (i removed the 90 degree addition from the -a1 near the end on purpose, it doesn’t change the result in a way I need)

1 Like

Hmm. I just edited some stuff in the IK module and now it works perfectly. I just made a1 positive and subtracted 90 degrees from a2-a1, seems to be working fine.

This isn’t really what i want though, I was the leg to lift up all the way and kinda crouch instead of kneel, so the knee is up and isn’t into the ground

I’d love to help but I’m not too keen on trig, so most of it is just going over my head

Well, this is the best result so far, thanks for helping though. I simply added a “base” value and did my calculations off of that angle, (math.pi/2) I also added 4 different positions, one being in between super close and perfect, so there weren’t that many artifacts.
https://streamable.com/p0f65e

This method of IK might not be ideal for what you’re trying to do. The method this thread depicts is pretty strict and what you want might be something more adaptive.

Adopting FABRIK style IK might be more what you’re looking for.

7 Likes

This is actually quite useful, however one issue I am having with it, is the hand touching the inner edge rather the centre.

How would I go about fixing this?

1 Like

It’s because the solve isn’t accounting for the outward offset the C1 of the shoulder has. Technically it’s correct since the line from the shoulder C0 to the point is still in line.

You can correct this by offsetting the originCF by the shoulder C1 offset outward (c1.X) or by rotating the final planeCF to compensate for the offset (think of the offset + arm lengths as a right angle triangle: tan(l1+l2 / c1.X)). Rotating means you’ll also need to adjust for the loss in distance by pushing the planeCF forwards.

4 Likes

`A = math.acos((-a^2 + b^2 - c^2) / (2 * b * c))`
wouldn’t it be:
` A = math.acos((-a^2 + b^2 + c^2) / (2 * b * c))`

2 Likes

This is a tutorial, not a project. This topic is intended to teach you the basics of IK, and it’s up to you to apply it yourself.

One case that needs to be adressed is when the `axis` is equal to `Vector3.new()`
A simple solution to this:

``````local axis = Vector3.new(0, 0, -1):Cross(localizedUnit)
local angle = math.acos(-localizedUnit.Z)
if axis == Vector3.new() then
axis = (angle == PI and Vector3.new(-1, 0, 0) or Vector3.new(0, 0, -1))
end
local planeCF = originCF * CFrame.fromAxisAngle(axis, angle)
``````

This addresses two very specific cases where the math breaks Just wanted to add this on.

3 Likes

No, the first equation is correct