Print warnings on solver/actuator failures

roboticslab-uc3m · Jul 17, 2018 · e76d3bf · e76d3bf
1 parent 3bb6d70
commit e76d3bf
Showing 1 changed file with 18 additions and 4 deletions.
diff --git a/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp
@@ -215,9 +215,16 @@ void roboticslab::BasicCartesianControl::handleGcmp()
 
     std::vector<double> t(numRobotJoints);
 
-    iCartesianSolver->invDyn(currentQ, t);
+    if (!iCartesianSolver->invDyn(currentQ, t))
+    {
+        CD_WARNING("invDyn failed, not updating control this iteration.\n");
+        return;
+    }
 
-    iTorqueControl->setRefTorques(t.data());
+    if (!iTorqueControl->setRefTorques(t.data()))
+    {
+        CD_WARNING("setRefTorques failed, not updating control this iteration.\n");
+    }
 }
 
 // -----------------------------------------------------------------------------
@@ -246,9 +253,16 @@ void roboticslab::BasicCartesianControl::handleForc()
 
     std::vector<double> t(numRobotJoints);
 
-    iCartesianSolver->invDyn(currentQ, qdot, qdotdot, fexts, t);
+    if (!iCartesianSolver->invDyn(currentQ, qdot, qdotdot, fexts, t))
+    {
+        CD_WARNING("invDyn failed, not updating control this iteration.\n");
+        return;
+    }
 
-    iTorqueControl->setRefTorques(t.data());
+    if (!iTorqueControl->setRefTorques(t.data()))
+    {
+        CD_WARNING("setRefTorques failed, not updating control this iteration.\n");
+    }
 }
 
 // -----------------------------------------------------------------------------