From a4cb7e6b9e91f5d9f7d14983883f78bcbb4e88eb Mon Sep 17 00:00:00 2001
From: Dan Royer <dan@marginallyclever.com>
Date: Sun, 7 Jan 2024 12:29:27 -0800
Subject: [PATCH] prevent drift when linear velocity=0

---
 .../java/com/marginallyclever/ro3/node/nodes/LimbSolver.java    | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/LimbSolver.java b/src/main/java/com/marginallyclever/ro3/node/nodes/LimbSolver.java
index 9596f46dc..9ad7965b1 100644
--- a/src/main/java/com/marginallyclever/ro3/node/nodes/LimbSolver.java
+++ b/src/main/java/com/marginallyclever/ro3/node/nodes/LimbSolver.java
@@ -63,6 +63,8 @@ private Pose getEndEffector() {
 
     private void moveTowardsTarget() {
         if(getLimb()==null || getEndEffector()==null || getTarget()==null || linearVelocity<0.0001) {
+            // no limb, no end effector, no target, or no velocity.  Do nothing.
+            getLimb().setAllJointVelocities(new double[getLimb().getNumJoints()]);
             return;
         }
         double[] cartesianVelocity = MatrixHelper.getCartesianBetweenTwoMatrices(