Revert "Prefer position direct mode in closed-loop movj command"

This reverts commit 3934932.
roboticslab-uc3m · Jan 10, 2020 · 0528d96 · 0528d96
1 parent 1d73397
commit 0528d96
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 17 deletions.
diff --git a/libraries/YarpPlugins/SoftNeckControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/SoftNeckControl/ICartesianControlImpl.cpp
@@ -65,27 +65,21 @@ bool SoftNeckControl::inv(const std::vector<double> & xd, std::vector<double> &
 
 bool SoftNeckControl::movj(const std::vector<double> & xd)
 {
-    if (serialPort.isClosed())
+    if (!setControlModes(VOCAB_CM_POSITION))
     {
-        if (!setControlModes(VOCAB_CM_POSITION))
-        {
-            CD_ERROR("Unable to set position mode.\n");
-            return false;
-        }
+        CD_ERROR("Unable to set position mode.\n");
+        return false;
+    }
 
+    if (serialPort.isClosed())
+    {
         if (!sendTargets(xd))
         {
             return false;
         }
     }
     else
     {
-        if (!setControlModes(VOCAB_CM_POSITION_DIRECT))
-        {
-            CD_ERROR("Unable to set position direct mode.\n");
-            return false;
-        }
-
         resetController();
 
         if (!decodePose(xd, targetPose, coordinate_system::NONE, orientation_system::POLAR_AZIMUTH, angular_units::DEGREES))

diff --git a/libraries/YarpPlugins/SoftNeckControl/PeriodicThreadImpl.cpp b/libraries/YarpPlugins/SoftNeckControl/PeriodicThreadImpl.cpp
@@ -60,19 +60,31 @@ void SoftNeckControl::handleMovjClosedLoop()
         CD_WARNING("Outdated serial stream data.\n");
     }
 
-    double error = targetPose[0] - x_imu[0];
+    std::vector<double> xd = targetPose;
+    double error = xd[0] - x_imu[0];
     double cs = error > *controller;
 
     if (!std::isnormal(cs))
     {
         cs = 0.0;
     }
 
-    CD_DEBUG("pitch: target %f, sensor %f, error %f, cs: %f\n", targetPose[0], x_imu[0], error, cs);
+    CD_DEBUG("pitch: target %f, sensor %f, error %f, cs: %f\n", xd[0], x_imu[0], error, cs);
 
-    std::vector<double> qd;
-    computeIk(cs, targetPose[1], qd);
-    iPositionDirect->setPositions(qd.data());
+    xd[0] = cs;
+
+    if (!encodePose(xd, xd, coordinate_system::NONE, orientation_system::POLAR_AZIMUTH, angular_units::DEGREES))
+    {
+        CD_ERROR("encodePose failed.\n");
+        cmcSuccess = false;
+        stopControl();
+        return;
+    }
+
+    if (!sendTargets(xd))
+    {
+        CD_WARNING("Command error, not updating control this iteration.\n");
+    }
 }
 
 // -----------------------------------------------------------------------------