From d75b8bc6bb558d60f0e1f23413fb477b4c39f6cf Mon Sep 17 00:00:00 2001 From: Patryk Kalinowski Date: Wed, 3 May 2023 23:43:32 +0200 Subject: [PATCH] basic implementation of PID controller for physics hands movement --- .../physics-movement/scenes/player.tscn | 1 + .../physics-movement/scripts/physics_hand.gd | 110 ++++++++++++++++-- project.godot | 1 + 3 files changed, 102 insertions(+), 10 deletions(-) diff --git a/addons/xr-kit/physics-movement/scenes/player.tscn b/addons/xr-kit/physics-movement/scenes/player.tscn index f4dfb23..2709b40 100644 --- a/addons/xr-kit/physics-movement/scenes/player.tscn +++ b/addons/xr-kit/physics-movement/scenes/player.tscn @@ -469,6 +469,7 @@ origin = NodePath("Body/XROrigin3D") camera = NodePath("Body/XROrigin3D/XRCamera3D") [node name="CinematicView" parent="." node_paths=PackedStringArray("xr_camera") instance=ExtResource("2_ao5mr")] +enabled = false xr_camera = NodePath("../Body/XROrigin3D/XRCamera3D") [node name="Body" type="CharacterBody3D" parent="." node_paths=PackedStringArray("origin", "camera", "physics_hand_left", "physics_hand_right")] diff --git a/addons/xr-kit/physics-movement/scripts/physics_hand.gd b/addons/xr-kit/physics-movement/scripts/physics_hand.gd index f6abdfb..e77d735 100644 --- a/addons/xr-kit/physics-movement/scripts/physics_hand.gd +++ b/addons/xr-kit/physics-movement/scripts/physics_hand.gd @@ -15,10 +15,17 @@ signal hand_reset(hand: Node3D) @export var wrist_raycast: RayCast3D # wrist raycasts detect objects to grab @export var wrist_joint: Generic6DOFJoint3D # joint is holding objects +@export_group("PID Controller Linear") +@export var Kp: float = 1800 +@export var Ki: float = 1 +@export var Kd: float = 10 +@export var integral_limit: float = 100 +@export var derivative_limit: float = 100 + +var pid_controller_linear: PIDController +var pid_controller_angular: PIDController var controller_hand_mesh_material: Material var held_object: Node3D = null -var acceleration: Vector3 -var angular_acceleration: Vector3 var trigger_pressed: bool var physics_pivot_point: Node3D var thruster_forward: bool @@ -69,6 +76,21 @@ func _ready() -> void: # that's why we add all finger colliders to it add_child(collider) + pid_controller_linear = PIDController.new({ + Kp = Kp, + Ki = Ki, + Kd = Kd, + integral_limit = integral_limit, + derivative_limit = derivative_limit + }) + pid_controller_angular = PIDController.new({ + Kp = 10, # must not exceed 10, higher values glitch physics + Ki = 0, + Kd = 0, + integral_limit = 1, + derivative_limit = 1 + }) + func _physics_process(delta: float) -> void: # process every bone in hand @@ -143,22 +165,21 @@ func process_bones(bone_id: int, delta: float) -> void: break func _move(delta: float) -> void: - # movement is based on wrist bone - var bone_id: int = 0 - # reset movement from previous frame, so hand doesn't overshoot + # reset movement from previous frame, for some reason this prevents ghosting set_linear_velocity(Vector3.ZERO) set_angular_velocity(Vector3.ZERO) - - var controller_bone_global_transform = controller_skeleton.global_transform * controller_skeleton.get_bone_global_pose(bone_id) + # target is controller wrist bone + var target = controller_skeleton.global_transform * controller_skeleton.get_bone_global_pose(0) # calculate acceleration needed to reach controller position in 1 frame - acceleration = 30 * (controller_bone_global_transform.origin - global_transform.origin) / delta + var linear_acceleration: Vector3 = pid_controller_linear.calculate(target.origin, global_transform.origin, delta) # multiplier on angular acceleration must be reduced to 5, higher values glitch the hand - angular_acceleration = 5 * (controller_bone_global_transform.basis * global_transform.basis.inverse()).get_euler() +# var angular_acceleration: Vector3 = pid_controller_angular.calculate((target.basis * global_transform.basis.inverse()).get_euler(), global_transform.basis.get_euler(), delta) + var angular_acceleration: Vector3 = pid_controller_angular.calculate((target.basis * global_transform.basis.inverse()).get_euler(), Vector3.ZERO, delta) # apply calculated forces # we include body's velocity so physics hand can follow controllers when player is moving fast - apply_central_force(acceleration + body.get_real_velocity() * 10) + apply_central_force(linear_acceleration) apply_torque(angular_acceleration) @@ -187,6 +208,21 @@ func grab() -> Node3D: held_object.set_center_of_mass_mode(1) # enable custom center of mass held_object.set_center_of_mass(center_of_mass) + pid_controller_linear.update({ + Kp = Kp, + Ki = Ki, + Kd = held_object.get_mass() * 2, + integral_limit = integral_limit, + derivative_limit = derivative_limit + }) + pid_controller_angular.update({ + Kp = 10, + Ki = 5, + Kd = 5, + integral_limit = 1, + derivative_limit = 1 + }) + held_object.set_collision_layer_value(12, true) # held objects are in layer 12 to filter out collisions with player head grabbed.emit(held_object) @@ -206,6 +242,21 @@ func drop_held_object() -> void: held_object.set_angular_damp(0) held_object.set_center_of_mass_mode(0) + pid_controller_linear.update({ + Kp = Kp, + Ki = Ki, + Kd = 10, + integral_limit = integral_limit, + derivative_limit = derivative_limit + }) + pid_controller_angular.update({ + Kp = 10, + Ki = 0, + Kd = 0, + integral_limit = 1, + derivative_limit = 1 + }) + dropped_held_object.emit(held_object) held_object = null @@ -256,3 +307,42 @@ func _on_hand_pose_recognition_new_pose(previous_pose: StringName, pose: StringN if pose in ["open", "rest"]: drop_held_object() + + +class PIDController: + var Kp: float + var Ki: float + var Kd: float + var error: Vector3 + var proportional: Vector3 + var previous_error: Vector3 + var integral: Vector3 + var integral_limit: float + var derivative: Vector3 + var output: Vector3 + + func _init(args) -> void: + Kp = args.Kp + Ki = args.Ki + Kd = args.Kd + integral_limit = args.integral_limit + previous_error = Vector3(0, 0, 0) + integral = Vector3(0, 0, 0) + + func update(args) -> void: + Kp = args.Kp + Ki = args.Ki + Kd = args.Kd + integral_limit = args.integral_limit + + + func calculate(target: Vector3, current: Vector3, delta: float) -> Vector3: + error = target - current + proportional = error + integral += error * delta + integral.limit_length(integral_limit) + derivative = (error - previous_error) / delta + previous_error = error + output = Kp * proportional + Ki * integral + Kd * derivative + + return output diff --git a/project.godot b/project.godot index e29ebeb..a050656 100644 --- a/project.godot +++ b/project.godot @@ -28,6 +28,7 @@ window/subwindows/embed_subwindows=false [physics] +common/physics_ticks_per_second=90 3d/default_gravity=0.0 [xr]