From b69b66c10b670dd807f605104b4e37a4aa3926a8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Mon, 1 Jan 2024 01:36:24 +0100
Subject: [PATCH] Enforce isochronous joint trajectories

---
 programs/BodyExecution/BodyExecution.cpp | 52 +++++++++++++++++++++++-
 programs/BodyExecution/BodyExecution.hpp |  8 +++-
 2 files changed, 56 insertions(+), 4 deletions(-)

diff --git a/programs/BodyExecution/BodyExecution.cpp b/programs/BodyExecution/BodyExecution.cpp
index ecb54aa..740df40 100644
--- a/programs/BodyExecution/BodyExecution.cpp
+++ b/programs/BodyExecution/BodyExecution.cpp
@@ -2,6 +2,9 @@
 
 #include "BodyExecution.hpp"
 
+#include <cmath> // std::abs
+
+#include <algorithm> // std::max_element, std::transform
 #include <vector>
 
 #include <yarp/os/LogStream.h>
@@ -58,7 +61,7 @@ bool BodyExecution::configure(yarp::os::ResourceFinder & rf)
         return false;
     }
 
-    if (!robotDevice.view(iControlMode) || !robotDevice.view(iPositionControl))
+    if (!robotDevice.view(iControlMode) || !robotDevice.view(iEncoders) || !robotDevice.view(iPositionControl))
     {
         yError() << "Failed to view robot interfaces";
         return false;
@@ -94,6 +97,12 @@ bool BodyExecution::configure(yarp::os::ResourceFinder & rf)
 bool BodyExecution::close()
 {
     stop();
+
+    if (int numAxes; !iEncoders->getAxes(&numAxes) || !iPositionControl->setRefSpeeds(std::vector(numAxes, DEFAULT_REF_SPEED).data()))
+    {
+        yWarning() << "Failed to restore reference speeds";
+    }
+
     serverPort.close();
     robotDevice.close();
     return true;
@@ -142,7 +151,7 @@ bool BodyExecution::updateModule()
         values.insert(values.end(), leftArm.cbegin(), leftArm.cend());
         values.insert(values.end(), rightArm.begin(), rightArm.end());
 
-        if (!iPositionControl->positionMove(values.data()))
+        if (!sendMotionCommand(values))
         {
             yWarning() << "Failed to send new setpoints";
         }
@@ -151,6 +160,45 @@ bool BodyExecution::updateModule()
     return true;
 }
 
+bool BodyExecution::sendMotionCommand(const std::vector<double> & targets)
+{
+    std::vector<double> q(targets.size());
+
+    if (!iEncoders->getEncoders(q.data()))
+    {
+        yWarning() << "Failed to get current encoder values";
+        return false;
+    }
+
+    std::vector<double> deltas(targets.size());
+
+    std::transform(targets.cbegin(), targets.cend(), q.cbegin(), deltas.begin(), [](auto target, auto current) {
+        return std::abs(target - current);
+    });
+
+    double maxDelta = *std::max_element(deltas.cbegin(), deltas.cend());
+
+    std::vector<double> refSpeeds(targets.size());
+
+    std::transform(deltas.cbegin(), deltas.cend(), refSpeeds.begin(), [maxDelta](auto delta) {
+        return DEFAULT_REF_SPEED * delta / maxDelta; // isochronous
+    });
+
+    if (!iPositionControl->setRefSpeeds(refSpeeds.data()))
+    {
+        yWarning() << "Failed to set reference speeds";
+        return false;
+    }
+
+    if (!iPositionControl->positionMove(targets.data()))
+    {
+        yWarning() << "Failed to send motion command";
+        return false;
+    }
+
+    return true;
+}
+
 void BodyExecution::doGreet()
 {
     registerSetpoints("greet", {
diff --git a/programs/BodyExecution/BodyExecution.hpp b/programs/BodyExecution/BodyExecution.hpp
index 075de3f..cd8b7cf 100644
--- a/programs/BodyExecution/BodyExecution.hpp
+++ b/programs/BodyExecution/BodyExecution.hpp
@@ -8,11 +8,13 @@
 #include <initializer_list>
 #include <mutex>
 #include <tuple>
+#include <vector>
 
 #include <yarp/os/RFModule.h>
 #include <yarp/os/RpcServer.h>
 
 #include <yarp/dev/IControlMode.h>
+#include <yarp/dev/IEncoders.h>
 #include <yarp/dev/IPositionControl.h>
 #include <yarp/dev/PolyDriver.h>
 
@@ -59,6 +61,7 @@ class BodyExecution : public yarp::os::RFModule,
 
 private:
     void registerSetpoints(const std::string & action, std::initializer_list<setpoints_t> setpoints);
+    bool sendMotionCommand(const std::vector<double> & targets);
 
     const std::string noAction { "none" };
 
@@ -68,8 +71,9 @@ class BodyExecution : public yarp::os::RFModule,
     bool isProcessingSetpoints { false };
 
     yarp::dev::PolyDriver robotDevice;
-    yarp::dev::IControlMode * iControlMode;
-    yarp::dev::IPositionControl * iPositionControl;
+    yarp::dev::IControlMode * iControlMode { nullptr };
+    yarp::dev::IEncoders * iEncoders { nullptr };
+    yarp::dev::IPositionControl * iPositionControl { nullptr };
 
     yarp::os::RpcServer serverPort;
 };