prevent drift when linear velocity=0

MarginallyClever · Jan 7, 2024 · a4cb7e6 · a4cb7e6
1 parent 79c1fd4
commit a4cb7e6
Showing 1 changed file with 2 additions and 0 deletions.
diff --git 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(