From d9c513bbd94e1e5c72f4c29bdce29e8f42f47f24 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Wed, 17 Jan 2024 19:36:43 +0100
Subject: [PATCH] Only command moving joints, avoid setting zero ref speeds

---
 programs/BodyExecution/BodyExecution.cpp | 39 ++++++++++++++++--------
 1 file changed, 26 insertions(+), 13 deletions(-)

diff --git a/programs/BodyExecution/BodyExecution.cpp b/programs/BodyExecution/BodyExecution.cpp
index 77b90ad..ccca0f9 100644
--- a/programs/BodyExecution/BodyExecution.cpp
+++ b/programs/BodyExecution/BodyExecution.cpp
@@ -175,24 +175,37 @@ bool BodyExecution::sendMotionCommand(const std::vector<double> & targets)
         return std::abs(target - current);
     });
 
-    double maxDelta = *std::max_element(deltas.cbegin(), deltas.cend());
+    if (double maxDelta = *std::max_element(deltas.cbegin(), deltas.cend()); maxDelta != 0.0)
+    {
+        std::vector<int> indices;
+        std::vector<double> refSpeeds;
+        std::vector<double> groupTargets;
 
-    std::vector<double> refSpeeds(targets.size());
+        for (auto i = 0; i < deltas.size(); i++)
+        {
+            if (deltas[i] != 0.0)
+            {
+                indices.push_back(i);
+                refSpeeds.push_back(DEFAULT_REF_SPEED * deltas[i] / maxDelta); // isochronous motion
+                groupTargets.push_back(targets[i]);
+            }
+        }
 
-    std::transform(deltas.cbegin(), deltas.cend(), refSpeeds.begin(), [maxDelta](auto delta) {
-        return DEFAULT_REF_SPEED * delta / maxDelta; // isochronous
-    });
+        if (!iPositionControl->setRefSpeeds(indices.size(), indices.data(), refSpeeds.data()))
+        {
+            yWarning() << "Failed to set reference speeds";
+            return false;
+        }
 
-    if (!iPositionControl->setRefSpeeds(refSpeeds.data()))
-    {
-        yWarning() << "Failed to set reference speeds";
-        return false;
+        if (!iPositionControl->positionMove(indices.size(), indices.data(), groupTargets.data()))
+        {
+            yWarning() << "Failed to send motion command";
+            return false;
+        }
     }
-
-    if (!iPositionControl->positionMove(targets.data()))
+    else
     {
-        yWarning() << "Failed to send motion command";
-        return false;
+        yWarning() << "No motion to perform, already on target";
     }
 
     return true;