From 8c3a53cb76d6315269fe19ec8e90088a99d32f41 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sun, 4 Aug 2024 16:40:25 -0400 Subject: [PATCH] Fixes and extra doc link --- src/pyroboplan/ik/differential_ik.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/pyroboplan/ik/differential_ik.py b/src/pyroboplan/ik/differential_ik.py index c0e3ec0..dac2925 100644 --- a/src/pyroboplan/ik/differential_ik.py +++ b/src/pyroboplan/ik/differential_ik.py @@ -87,6 +87,7 @@ class DifferentialIk: * https://motion.cs.illinois.edu/RoboticSystems/InverseKinematics.html * https://homes.cs.washington.edu/~todorov/courses/cseP590/06_JacobianMethods.pdf * https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf + * http://www.diag.uniroma1.it/deluca/rob2_en/02_KinematicRedundancy_1.pdf """ def __init__( @@ -180,9 +181,7 @@ def solve( else: W = np.diag(self.options.joint_weights) # Invert the original weight matrix so that higher weight means less joint motion. - # Then, normalize it to protect against numerical instabilities. W = np.linalg.inv(W) - W /= np.max(W) # Create a random initial state, if not specified if init_state is None: @@ -267,7 +266,7 @@ def solve( # Gradient descent step if not nullspace_components: - q_step = alpha * J.T @ np.linalg.solve(jjt, error) + q_step = alpha * W @ J.T @ np.linalg.solve(jjt, error) else: nullspace_term = sum( [ @@ -276,7 +275,7 @@ def solve( ] ) q_step = alpha * ( - J.T @ (np.linalg.solve(jjt, error - J @ (nullspace_term))) + W @ J.T @ (np.linalg.solve(jjt, error - J @ (nullspace_term))) + nullspace_term )