Robotic Arm Inverse Kinematics

Help and Feedback Scripting Support
#1

Basically the rundown here is I’m replicating the kinematic system of the Canada Arm which operates on the principle of adjusting joint angles to position the end effector at a specific Vector3 value.

I have it working when the arm is operating from a level base as such: https://gyazo.com/8dbe298712a459bfec14b4c6e5784c7e

However when the base is rotated as displayed in the following gif, the arm is offset from the actual point it is meant to be following.

https://gyazo.com/7c018f46cf0baae323e0cfd01c4de93b

I’m not the best with inverse kinematics and this is really my first experiment with the concept. Any ideas how to simply this function or correct any calculation errors to prevent this undesired effect?

The function for the entire arm is listed below:

function updatejoints()
–//Shoulder Rotation
local differencez = script.Parent.ShoulderBase.CFrame:PointToObjectSpace(script.Parent.WristPos.Position)
joints.ShoulderRotation.C0 = CFrame.Angles(0,0,-(math.rad(90) - math.atan2(differencez.Z,differencez.X)))

--//Vertical Computation
local adjustment = CFrame.new(script.Parent.ShoulderBase2.Position, script.Parent.WristPos.Position)
script.Parent.ShoulderBase2.CFrame = adjustment
local adjustangle = ((math.rad(script.Parent.ShoulderBase2.Orientation.X)))

--//Shoulder Pitch
local armlength = ((script.Parent.ShoulderBase.Position - script.Parent.WristPos.Position).Magnitude)
local lowerarmlength = ((script.Parent.ShoulderBase.Position - script.Parent.UpperArm.Elbow.Base.Position).Magnitude)
local upperarmlength = ((script.Parent.UpperArm.Elbow.Base.Position - script.Parent.Wrist2.Base.Position).Magnitude)
local rad = ((lowerarmlength^2+armlength^2-upperarmlength^2)/(2*(lowerarmlength*armlength)))
local shoulderangle = math.acos(rad)
script.Parent.Joints.ShoulderPitch.C0 = CFrame.Angles(0,0,(shoulderangle+adjustangle))


--//Elbow (calculates opposite and then subtracts both from 180 to find elbow)
local rad2 = ((armlength^2+upperarmlength^2-lowerarmlength^2)/(2*(armlength*upperarmlength)))	
local oppositeshoulderangle = math.acos(rad2)
local elbowangle = math.rad(180-math.deg(shoulderangle)-math.deg(oppositeshoulderangle))
script.Parent.Joints.Elbow.C0 = CFrame.Angles(0,0,math.rad(180)+elbowangle)

end

3 Likes
#2

I transformed your code into a format that made more sense to me. I had to guess what most parts are, could you provide an explanation of the parts involved and their orientations? Otherwise, I’m not immediately seeing any issues.

local up = Vector3.new(0, 1, 0)
local function lawOfCosines(a, b, c)
	return acos((a*a + b*b - c*c) / (2 * a * b))
end
function updatejoints()
	--//Shoulder Rotation
	local base_wrist = script.Parent.ShoulderBase.CFrame:Inverse() * script.Parent.WristPos.Position
	local aZ = math.atan2(base_wrist.Z, base_wrist.X) - math.pi/2
	joints.ShoulderRotation.C0 = CFrame.Angles(0, 0, aZ))

	--//Vertical Computation
	local s2 = script.Parent.ShoulderBase2
	s2.CFrame = CFrame.new(
		s2.Position,
		script.Parent.WristPos.Position
	)

	local s2_wrist = script.Parent.WristPos.Position - script.Parent.ShoulderBase2.Position
	local angleOfDepression = math.pi/2 - math.acos(up:Dot(s2_wrist.Unit))

	--//Shoulder Pitch
	local a = (script.Parent.ShoulderBase.Position - script.Parent.WristPos.Position).Magnitude
	local b = (script.Parent.ShoulderBase.Position - script.Parent.UpperArm.Elbow.Base.Position).Magnitude
	local c = (script.Parent.UpperArm.Elbow.Base.Position - script.Parent.Wrist2.Base.Position).Magnitude

	local C = lawOfCosines(a, b, c)
	local A = lawOfCosines(b, c, a)

	script.Parent.Joints.ShoulderPitch.C0 = CFrame.Angles(0, 0, angleOfDepression + C)
	script.Parent.Joints.Elbow.C0 = CFrame.Angles(0, 0, math.pi + A)
end
Rotational point between two parts?
#4

Pretty sure this is your problem, as you offset the ShoulderAngle only by the Base’s X orientation. Just calculate the angle between the level plane and the plane of the Base instead of using Orientation.X

#5

I was suspicious of this line as well, but for different reasons. First, there are many ways to represent a CFrame in Eular angles and so the X component may never actually change (hence why I choose a different method to calculate it). I believe the current implementation of the CFrame look at constructor attempts to keep the x axis orthogonal to the global y axis, so this may not be an issue in this case. Second, the angle / offset needed is based upon how the joints between parts are configured and their starting CFrames. We need more information.