Don't abort MOVJ if any ref speed is zero

roboticslab-uc3m · Jan 17, 2024 · 30326c3 · 30326c3
1 parent 1facb7c
commit 30326c3
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 36 deletions.
diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp
@@ -266,9 +266,9 @@ bool BasicCartesianControl::presetStreamingCommand(int command)
 
 // -----------------------------------------------------------------------------
 
-void BasicCartesianControl::computeIsocronousSpeeds(const std::vector<double> & q, const std::vector<double> & qd,
-        std::vector<double> & qdot)
+bool BasicCartesianControl::computeIsocronousSpeeds(const std::vector<double> & q, const std::vector<double> & qd, std::vector<double> & qdot)
 {
+    std::vector<double> deltas(numJoints);
     double maxTime = 0.0;
 
     //-- Find out the maximum time to move
@@ -278,14 +278,14 @@ void BasicCartesianControl::computeIsocronousSpeeds(const std::vector<double> &
         if (qRefSpeeds[joint] <= 0.0)
         {
             yCWarning(BCC, "Zero or negative velocities sent at joint %d, not moving: %f", joint, qRefSpeeds[joint]);
-            return;
+            continue;
         }
 
-        double distance = std::abs(qd[joint] - q[joint]);
+        deltas[joint] = std::abs(qd[joint] - q[joint]);
 
-        yCInfo(BCC, "Distance (joint %d): %f", joint, distance);
+        yCInfo(BCC, "Distance (joint %d): %f", joint, deltas[joint]);
 
-        double targetTime = distance / qRefSpeeds[joint];
+        double targetTime = deltas[joint] / qRefSpeeds[joint];
 
         if (targetTime > maxTime)
         {
@@ -300,14 +300,17 @@ void BasicCartesianControl::computeIsocronousSpeeds(const std::vector<double> &
     {
         if (maxTime == 0.0)
         {
+            qdot[joint] = 0.0;
             yCInfo(BCC, "qdot[%d] = 0.0 (forced)", joint);
         }
         else
         {
-            qdot[joint] = std::abs(qd[joint] - q[joint]) / maxTime;
+            qdot[joint] = deltas[joint] / maxTime;
             yCInfo(BCC, "qdot[%d] = %f", joint, qdot[joint]);
         }
     }
+
+    return maxTime != 0.0; // true: will move
 }
 
 // -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp
@@ -168,7 +168,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
     bool checkControlModes(int mode);
     bool setControlModes(int mode);
     bool presetStreamingCommand(int command);
-    void computeIsocronousSpeeds(const std::vector<double> & q, const std::vector<double> & qd, std::vector<double> & qdot);
+    bool computeIsocronousSpeeds(const std::vector<double> & q, const std::vector<double> & qd, std::vector<double> & qdot);
 
     void handleMovj(const std::vector<double> & q, const StateWatcher & watcher);
     void handleMovlVel(const std::vector<double> & q, const StateWatcher & watcher);

diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp
@@ -102,42 +102,46 @@ bool BasicCartesianControl::movj(const std::vector<double> &xd)
         return false;
     }
 
-    std::vector<double> vmo(numJoints);
+    if (std::vector<double> vmo(numJoints); computeIsocronousSpeeds(currentQ, qd, vmo))
+    {
+        vmoStored.resize(numJoints);
 
-    computeIsocronousSpeeds(currentQ, qd, vmo);
-    vmoStored.resize(numJoints);
+        if (!iPositionControl->getRefSpeeds(vmoStored.data()))
+        {
+            yCError(BCC) << "getRefSpeeds() (for storing) failed";
+            return false;
+        }
 
-    if (!iPositionControl->getRefSpeeds(vmoStored.data()))
-    {
-         yCError(BCC) << "getRefSpeeds() (for storing) failed";
-         return false;
-    }
+        if (!iPositionControl->setRefSpeeds(vmo.data()))
+        {
+            yCError(BCC) << "setRefSpeeds() failed";
+            return false;
+        }
 
-    if (!iPositionControl->setRefSpeeds(vmo.data()))
-    {
-         yCError(BCC) << "setRefSpeeds() failed";
-         return false;
-    }
+        //-- Enter position mode and perform movement
+        if (!setControlModes(VOCAB_CM_POSITION))
+        {
+            yCError(BCC) << "Unable to set position mode";
+            return false;
+        }
 
-    //-- Enter position mode and perform movement
-    if (!setControlModes(VOCAB_CM_POSITION))
-    {
-        yCError(BCC) << "Unable to set position mode";
-        return false;
-    }
+        if (!iPositionControl->positionMove(qd.data()))
+        {
+            yCError(BCC) << "positionMove() failed";
+            return false;
+        }
+
+        //-- Set state, enable CMC thread and wait for movement to be done
+        cmcSuccess = true;
+        yCInfo(BCC) << "Performing MOVJ";
 
-    if (!iPositionControl->positionMove(qd.data()))
+        setCurrentState(VOCAB_CC_MOVJ_CONTROLLING);
+    }
+    else
     {
-        yCError(BCC) << "positionMove() failed";
-        return false;
+        yCWarning(BCC) << "No motion planned";
     }
 
-    //-- Set state, enable CMC thread and wait for movement to be done
-    cmcSuccess = true;
-    yCInfo(BCC) << "Performing MOVJ";
-
-    setCurrentState(VOCAB_CC_MOVJ_CONTROLLING);
-
     return true;
 }