From 30326c3a77027d13840f561c6e862e4da6b0377f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Wed, 17 Jan 2024 22:49:25 +0100 Subject: [PATCH] Don't abort MOVJ if any ref speed is zero --- .../BasicCartesianControl.cpp | 17 +++--- .../BasicCartesianControl.hpp | 2 +- .../ICartesianControlImpl.cpp | 60 ++++++++++--------- 3 files changed, 43 insertions(+), 36 deletions(-) diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp index f77081e63..0bddc7e1f 100644 --- 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 & q, const std::vector & qd, - std::vector & qdot) +bool BasicCartesianControl::computeIsocronousSpeeds(const std::vector & q, const std::vector & qd, std::vector & qdot) { + std::vector deltas(numJoints); double maxTime = 0.0; //-- Find out the maximum time to move @@ -278,14 +278,14 @@ void BasicCartesianControl::computeIsocronousSpeeds(const std::vector & 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 & { 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 index 2e7496c78..9ddc658dd 100644 --- 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 & q, const std::vector & qd, std::vector & qdot); + bool computeIsocronousSpeeds(const std::vector & q, const std::vector & qd, std::vector & qdot); void handleMovj(const std::vector & q, const StateWatcher & watcher); void handleMovlVel(const std::vector & q, const StateWatcher & watcher); diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index 6f6684f57..bea641500 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -102,42 +102,46 @@ bool BasicCartesianControl::movj(const std::vector &xd) return false; } - std::vector vmo(numJoints); + if (std::vector 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; }