Retrieve, store and use joint speed limits

roboticslab-uc3m · May 16, 2019 · a3600f7 · a3600f7
1 parent 4fe827f
commit a3600f7
Show file tree

Hide file tree

Showing 6 changed files with 47 additions and 14 deletions.
diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp
@@ -12,9 +12,9 @@ bool roboticslab::AmorCartesianControl::checkJointVelocities(const std::vector<d
 {
     for (unsigned int i = 0; i < qdot.size(); i++)
     {
-        if (std::abs(qdot[i]) > 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
@@ -3,6 +3,8 @@
 #ifndef __AMOR_CARTESIAN_CONTROL_HPP__
 #define __AMOR_CARTESIAN_CONTROL_HPP__
 
+#include <vector>
+
 #include <yarp/os/Searchable.h>
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/PolyDriver.h>
@@ -127,6 +129,8 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
     double maxJointVelocity;
     int waitPeriodMs;
 
+    std::vector<double> qdotMax;
+
     ICartesianSolver::reference_frame referenceFrame;
 };
 

diff --git 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
@@ -56,7 +56,8 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector<doub
         // https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/161#issuecomment-428133287
         if (value < qMin[joint] + epsilon || value > 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<doub
 
         if (value < qMin[joint] || value > 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<doub
 
 bool roboticslab::BasicCartesianControl::checkJointVelocities(const std::vector<double> &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
@@ -271,6 +271,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
     bool cmcSuccess;
 
     std::vector<double> qMin, qMax;
+    std::vector<double> qdotMin, qdotMax;
 };
 
 }  // namespace roboticslab

diff --git 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;