From e76d3bf7801869ee9adfcb4566959c1954af8498 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Tue, 17 Jul 2018 17:46:30 +0200 Subject: [PATCH] Print warnings on solver/actuator failures --- .../BasicCartesianControl/RateThreadImpl.cpp | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp index b1242758d..3466e7dda 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp @@ -215,9 +215,16 @@ void roboticslab::BasicCartesianControl::handleGcmp() std::vector 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 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"); + } } // -----------------------------------------------------------------------------