Merge branch 'bcc-joint-limits' into develop

roboticslab-uc3m · Oct 16, 2018 · d2afc5e · d2afc5e
2 parents ed165cf + d678989
commit d2afc5e
Show file tree

Hide file tree

Showing 3 changed files with 65 additions and 6 deletions.
diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp
@@ -202,6 +202,8 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC
 
     protected:
 
+        bool checkJointLimits();
+
         void handleMovj();
         void handleMovl();
         void handleMovv();
@@ -224,6 +226,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC
         double gain;
         double maxJointVelocity;
         double duration; // [s]
+
         int cmcRateMs;
         int waitPeriodMs;
         int numRobotJoints, numSolverJoints;
@@ -251,6 +254,8 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC
         std::vector<double> td;
 
         bool cmcSuccess;
+
+        std::vector<double> qMin, qMax;
 };
 
 }  // namespace roboticslab

diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
@@ -73,21 +73,27 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config)
     iEncoders->getAxes(&numRobotJoints);
     CD_INFO("numRobotJoints: %d.\n",numRobotJoints);
 
-    yarp::os::Bottle qMin, qMax;
+    qMin.resize(numRobotJoints);
+    qMax.resize(numRobotJoints);
+
+    yarp::os::Bottle bMin, bMax;
+
     for(unsigned int joint=0;joint<numRobotJoints;joint++)
     {
         double min, max;
         iControlLimits->getLimits(joint,&min,&max);
-        qMin.addDouble(min);
-        qMax.addDouble(max);
+        qMin[joint] = min;
+        qMax[joint] = max;
+        bMin.addDouble(min);
+        bMax.addDouble(max);
         CD_INFO("Joint %d limits: [%f,%f]\n",joint,min,max);
     }
 
     yarp::os::Property solverOptions;
     solverOptions.fromString( config.toString() );
     solverOptions.put("device",solverStr);
-    solverOptions.put("mins", yarp::os::Value::makeList(qMin.toString().c_str()));
-    solverOptions.put("maxs", yarp::os::Value::makeList(qMax.toString().c_str()));
+    solverOptions.put("mins", yarp::os::Value::makeList(bMin.toString().c_str()));
+    solverOptions.put("maxs", yarp::os::Value::makeList(bMax.toString().c_str()));
     solverOptions.setMonitor(config.getMonitor(), solverStr.c_str());
 
     solverDevice.open(solverOptions);

diff --git a/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp
@@ -6,11 +6,59 @@
 #include <yarp/os/Time.h>
 #include <ColorDebug.h>
 
+namespace
+{
+    double epsilon = 1e-5;
+}
+
 // ------------------- RateThread Related ------------------------------------
 
+bool roboticslab::BasicCartesianControl::checkJointLimits()
+{
+    std::vector<double> currentQ(numRobotJoints);
+
+    if (!iEncoders->getEncoders(currentQ.data()))
+    {
+        CD_WARNING("getEncoders failed, unable to check joint limits.\n");
+        return false;
+    }
+
+    for (unsigned int joint = 0; joint < numRobotJoints; joint++)
+    {
+        double value = currentQ[joint];
+
+        // Report limit before reaching the actual value.
+        // https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/161#issuecomment-428133287
+        if (value < qMin[joint] + epsilon || value > qMax[joint] - epsilon)
+        {
+            CD_WARNING("Joint q%d out of limits [%f,%f]: %f.\n", joint + 1, qMin[joint], qMax[joint], value);
+            return false;
+        }
+    }
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
 void roboticslab::BasicCartesianControl::run()
 {
-    switch (getCurrentState())
+    const int currentState = getCurrentState();
+
+    if (currentState == VOCAB_CC_NOT_CONTROLLING)
+    {
+        return;
+    }
+
+    if (!checkJointLimits())
+    {
+        CD_ERROR("checkJointLimits failed, stopping control.\n");
+        cmcSuccess = false;
+        stopControl();
+        return;
+    }
+
+    switch (currentState)
     {
     case VOCAB_CC_MOVJ_CONTROLLING:
         handleMovj();