diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp index b687ad7d4..3304b1301 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp @@ -12,9 +12,9 @@ bool roboticslab::AmorCartesianControl::checkJointVelocities(const std::vector maxJointVelocity) + if (std::abs(qdot[i]) > qdotMax[i]) { - CD_ERROR("Maximum angular velocity hit: qdot[%d] = %f > %f [deg/s].\n", i, qdot[i], maxJointVelocity); + CD_ERROR("Maximum angular velocity hit: qdot[%d] = %f > %f [deg/s].\n", i, qdot[i], qdotMax[i]); return false; } } diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index 6e4454f19..71dac3607 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp @@ -3,6 +3,8 @@ #ifndef __AMOR_CARTESIAN_CONTROL_HPP__ #define __AMOR_CARTESIAN_CONTROL_HPP__ +#include + #include #include #include @@ -127,6 +129,8 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo double maxJointVelocity; int waitPeriodMs; + std::vector qdotMax; + ICartesianSolver::reference_frame referenceFrame; }; diff --git a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp index 621a4109a..17f1d04fe 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp @@ -74,6 +74,8 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config) CD_SUCCESS("Acquired AMOR handle!\n"); + qdotMax.resize(AMOR_NUM_JOINTS); + yarp::os::Bottle qMin, qMax; for (int i = 0; i < AMOR_NUM_JOINTS; i++) @@ -87,6 +89,8 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config) return false; } + qdotMax[i] = KinRepresentation::radToDeg(jointInfo.maxVelocity); + qMin.addDouble(KinRepresentation::radToDeg(jointInfo.lowerJointLimit)); qMax.addDouble(KinRepresentation::radToDeg(jointInfo.upperJointLimit)); } diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp index ad8b53177..d051ccae0 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp @@ -56,7 +56,8 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector qMax[joint] - epsilon) { - CD_WARNING("Joint near or out of limits: q[%d] = %f not in [%f,%f].\n", joint, value, qMin[joint], qMax[joint]); + CD_WARNING("Joint near or out of limits: q[%d] = %f not in [%f,%f] (def).\n", + joint, value, qMin[joint], qMax[joint]); return false; } } @@ -74,7 +75,8 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector qMax[joint]) { - CD_WARNING("Joint near or out of limits: q[%d] = %f not in [%f,%f].\n", joint, value, qMin[joint], qMax[joint]); + CD_WARNING("Joint near or out of limits: q[%d] = %f not in [%f,%f] (deg).\n", + joint, value, qMin[joint], qMax[joint]); double midRange = (qMax[joint] + qMin[joint]) / 2; // Let the joint get away from its nearest limit. @@ -92,11 +94,14 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector &qdot) { - for (unsigned int i = 0; i < qdot.size(); i++) + for (unsigned int joint = 0; joint < qdot.size(); joint++) { - if (std::abs(qdot[i]) > maxJointVelocity) + double value = qdot[joint]; + + if (value < qdotMin[joint] || value > qdotMax[joint]) { - CD_WARNING("Maximum angular velocity hit: qdot[%d] = |%f| > %f [deg/s].\n", i, qdot[i], maxJointVelocity); + CD_WARNING("Maximum angular velocity hit: qdot[%d] = %f not in [%f,%f] (deg/s).\n", + joint, value, qdotMin[joint], qdotMax[joint]); return false; } } diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index 261d40e07..90e2ebe8d 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -271,6 +271,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, bool cmcSuccess; std::vector qMin, qMax; + std::vector qdotMin, qdotMax; }; } // namespace roboticslab diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp index 003886460..ad8b12fff 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp @@ -112,20 +112,39 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config) qMin.resize(numRobotJoints); qMax.resize(numRobotJoints); + qdotMin.resize(numRobotJoints); + qdotMax.resize(numRobotJoints); + yarp::os::Bottle bMin, bMax; for (int joint = 0; joint < numRobotJoints; joint++) { - double min, max; - iControlLimits->getLimits(joint, &min, &max); + double _qMin, _qMax; + + if (!iControlLimits->getLimits(joint, &_qMin, &_qMax)) + { + CD_ERROR("Unable to retrieve position limits for joint %d.\n"); + return false; + } + + qMin[joint] = _qMin; + qMax[joint] = _qMax; + + double _qdotMin, _qdotMax; + + if (!iControlLimits->getVelLimits(joint, &_qdotMin, &_qdotMax)) + { + CD_ERROR("Unable to retrieve speed limits for joint %d.\n"); + return false; + } - qMin[joint] = min; - qMax[joint] = max; + qdotMin[joint] = _qdotMin; + qdotMax[joint] = _qdotMax; - bMin.addDouble(min); - bMax.addDouble(max); + CD_INFO("Joint %d limits: [%f,%f] [%f,%f]\n", joint, _qMin, _qMax, _qdotMin, _qdotMax); - CD_INFO("Joint %d limits: [%f,%f]\n", joint, min, max); + bMin.addDouble(_qMin); + bMax.addDouble(_qMax); } yarp::os::Property solverOptions;