From 5fc648faf899e9b55a4432250165b4ad04a3c618 Mon Sep 17 00:00:00 2001 From: KanishkNavale Date: Sun, 4 Feb 2024 22:49:06 +0100 Subject: [PATCH] Added: Newton decay to geodesic IK+ --- heimdall/robotics/inverse_kinematics.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/heimdall/robotics/inverse_kinematics.py b/heimdall/robotics/inverse_kinematics.py index 4f487ac..0e3a03e 100644 --- a/heimdall/robotics/inverse_kinematics.py +++ b/heimdall/robotics/inverse_kinematics.py @@ -133,12 +133,15 @@ def compute_geodesic_trajectory( jacobian = jacobian_function(q_current) p_current = forward_kinematics_function(q_current) + smooth_factor = pose_geodesic_distance(p_target, p_current) jacobian_inverse = compute_stable_inverse_jacobian(jacobian) - q_next = q_current + jacobian_inverse @ relative_pose(p_current, p_target) + q_next = q_current + smooth_factor * jacobian_inverse @ relative_pose( + p_current, p_target + ) joint_trajectory.append(q_next) - pose_distance = pose_geodesic_distance(p_target, p_current) + pose_distance = smooth_factor if torch.allclose(pose_distance, torch.zeros_like(pose_distance), atol=1e-4): break