Issues adjusting 4-bar linkage with HingeJoint3D

Godot Version



Hi, I’ve been trying to make a simple 4-bar linkage simulator in Godot. I was able to make a linkage in which all links were the same length and then I wrote up some code that I could attach to the parent node in order to be able to change the link lengths of individual links and starting angle of one of the links and then calculate where all the other links need to start (code is pasted below). I tested it by making one of the links (Link 1) have a length of 3 and the others have a length of 2. From the gif below, it looks like the links and joints begin in the correct place but then Link 2 (the link on the right that starts near vertically), snaps back to the position it would be in as if the length of Link 1 had been 2 (which is what it is by default before I change it with the @export variable.). Let me know if any other info would be useful. Thanks!


extends Node3D

# Only inputs are linkage lengths, starting angle
# Later will add motor parameters.
@export var link1_length: float = 2
@export var link2_length: float = 2
@export var link3_length: float = 2
@export var link4_length: float = 2
@export var input_angle: float = 90
@onready var link_lengths = [link1_length, link2_length, link3_length, link4_length]

# Called when the node enters the scene tree for the first time.
func _ready():
	var A = link1_length
	var B = link2_length
	var C = link3_length
	var D = link4_length

	var theta = deg_to_rad(input_angle)
	var psi = PI - theta
	var L = sqrt(A**2 + B**2 - 2 * A * B * cos(psi))
	var alpha = acos((L**2 + B**2 - A**2) / (2 * L * B))
	var beta = acos((L**2 + C**2 - D**2) / (2 * L * C))
	var lambda = (PI-alpha-beta) + theta
	var gamma = (PI - acos((C**2 + D**2 - L**2) / (2 * C * D))) + lambda
	var pos0 = Vector3.ZERO
	var pos1 = Vector3(A,0,0)
	var pos2 = pos1 + Vector3(B * cos(theta),
								B * sin(theta),
	var pos3 = pos2 + Vector3(C * cos(lambda),
								C * sin(lambda),
	var pos4 = pos3 + Vector3(D * cos(gamma),
								D * sin(gamma),
	var joint_positions = [pos0, pos1, pos2, pos3]
	var link_angles = [0.0, theta, lambda, gamma]
	var i = 0
	for link in self.get_children():

		if link is RigidBody3D:
			link.position = joint_positions[i]
			link.rotation = Vector3(0,0,link_angles[i])

			for node in link.get_children():

				if node is MeshInstance3D:
					var mesh = node.get_mesh()

					if mesh is CapsuleMesh:
						mesh.height = link_lengths[i]
						node.position = Vector3(link_lengths[i]/2,0,0)

				if node is CollisionShape3D:
					var shape = node.get_shape()
					shape.height = link_lengths[i]
					node.position = Vector3(link_lengths[i]/2,0,0)
				if node is HingeJoint3D:
					node.position = Vector3(link_lengths[i],0,0)


# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(delta):
	print("Link 2 position: %s" % $Link2.position[0])
	print("Joint 1 position: %s" % $Link1/Joint1.position[0])

Also, here is a screenshot of the node structure in case that is helpful.