Godot Version
4.2.1
Question
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():
## CALCULATE LINK POSITION AND ORIENTATIONS
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),
0)
var pos3 = pos2 + Vector3(C * cos(lambda),
C * sin(lambda),
0)
var pos4 = pos3 + Vector3(D * cos(gamma),
D * sin(gamma),
0)
var joint_positions = [pos0, pos1, pos2, pos3]
var link_angles = [0.0, theta, lambda, gamma]
# APPLYING LINK LENGTHS AND JOINT POSITION
var i = 0
for link in self.get_children():
if link is RigidBody3D:
#print(link)
link.position = joint_positions[i]
link.rotation = Vector3(0,0,link_angles[i])
#print(link.position)
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)
i+=1
# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(delta):
# CODE FOR MONITORING
print("Link 2 position: %s" % $Link2.position[0])
print("Joint 1 position: %s" % $Link1/Joint1.position[0])
