Fetch limits from robot, set in solver device

#145

Author: PeterBowman

programs/transCoords/TransCoords.cpp

@@ -5,6 +5,7 @@
 
 #include <yarp/os/Value.h>
 #include <yarp/os/Property.h>
+#include <yarp/dev/ControlBoardInterfaces.h>
 
 #include <ColorDebug.hpp>
 
@@ -90,6 +91,29 @@ bool TransCoords::configure(yarp::os::ResourceFinder &rf)
             return false;
         }
 
+        yarp::dev::IControlLimits * iControlLimits;
+
+        if (!robotDevice.view(iControlLimits))
+        {
+            CD_ERROR("Could not view iControlLimits.\n");
+            return false;
+        }
+
+        std::vector<double> qMin, qMax;
+
+        for (int i = 0; i < numRobotJoints; i++)
+        {
+            double min, max;
+            iControlLimits->getLimits(i, &min, &max);
+            qMin.push_back(min);
+            qMax.push_back(max);
+        }
+
+        if (qMin[0] != qMax[0])
+        {
+            iCartesianSolver->setLimits(qMin, qMax);
+        }
+
         CD_SUCCESS("numRobotJoints: %d.\n", numRobotJoints);
     }
     else