From e82b61c643dc3bf5273fb95f9d25b8ce9b796cd1 Mon Sep 17 00:00:00 2001
From: Davide De Tommaso <dtmdvd@gmail.com>
Date: Mon, 27 Jan 2025 15:52:27 +0100
Subject: [PATCH] Fixed move in position controller to allow req_time and
 timeout

---
 pyicub/controllers/position.py | 39 +++++++++++++++++++++++-----------
 1 file changed, 27 insertions(+), 12 deletions(-)

diff --git a/pyicub/controllers/position.py b/pyicub/controllers/position.py
index 42d16b3..8cfdd52 100755
--- a/pyicub/controllers/position.py
+++ b/pyicub/controllers/position.py
@@ -109,7 +109,7 @@ def getDriver(self):
 
 class PositionController:
 
-    WAITMOTIONDONE_PERIOD = 0.1
+    WAITMOTIONDONE_PERIOD = 0.01
     MOTION_COMPLETE_AT = 0.90
 
     def __init__(self, robot_name, part, logger):
@@ -150,27 +150,41 @@ def isMoving(self):
         return not self.__IPositionControl__.checkMotionDone()
 
     def __move__(self, target_joints, joints_list, req_time, joints_speed):
+
         disp  = [0]*len(joints_list)
         speeds = [0]*len(joints_list)
         times = [0]*len(joints_list)
         tmp   = yarp.Vector(self.__joints__)
         encs  = yarp.Vector(self.__joints__)
+
         while not self.__IEncoders__.getEncoders(encs.data()):
             yarp.delay(0.1)
         i = 0
-        for j in joints_list:
-            tmp.set(i, target_joints[i])
-            disp[i] = float(target_joints[i] - encs[j])
-            if disp[i] < 0.0:
-                disp[i] =- disp[i]
-            if abs(disp[i]) > 0.001:
-                speeds[i] = joints_speed[i]
-                times[i] = disp[i]/speeds[i]
+
+        if req_time > 0.0:
+            for j in joints_list:
+                tmp.set(i, target_joints[i])
+                disp[i] = target_joints[i] - encs[j]
+                if disp[i] < 0.0:
+                    disp[i] =- disp[i]
+                speeds[i] = disp[i]/req_time
                 self.__IPositionControl__.setRefSpeed(j, speeds[i])
                 self.__IPositionControl__.positionMove(j, tmp[i])
-            i+=1
-        if req_time == 0:
+                i+=1
+        else:
+            for j in joints_list:
+                tmp.set(i, target_joints[i])
+                disp[i] = float(target_joints[i] - encs[j])
+                if disp[i] < 0.0:
+                    disp[i] =- disp[i]
+                if abs(disp[i]) > 0.001:
+                    speeds[i] = joints_speed[i]
+                    times[i] = disp[i]/speeds[i]
+                    self.__IPositionControl__.setRefSpeed(j, speeds[i])
+                    self.__IPositionControl__.positionMove(j, tmp[i])
+                i+=1
             req_time = max(times)
+        
         return req_time
 
     def stop(self, joints_list=None):
@@ -283,11 +297,12 @@ def unsetCustomWaitMotionDone(self):
     def waitMotionDone(self, req_time: float=DEFAULT_TIMEOUT, timeout: float=DEFAULT_TIMEOUT):
         t0 = time.perf_counter()
         elapsed_time = 0.0
-        while elapsed_time < req_time:
+        while elapsed_time < timeout:
             if not self.isMoving():
                 return True
             yarp.delay(PositionController.WAITMOTIONDONE_PERIOD)
             elapsed_time = time.perf_counter() - t0
+        
         return False
 
     def waitMotionDone2(self, req_time: float=DEFAULT_TIMEOUT, timeout: float=DEFAULT_TIMEOUT):