Merge branch 'movi-apps' into develop

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;
         }
     }

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>
@@ -16,7 +18,6 @@
 #define DEFAULT_CAN_PORT 0
 
 #define DEFAULT_GAIN 0.05
-#define DEFAULT_QDOT_LIMIT 10.0
 #define DEFAULT_WAIT_PERIOD_MS 30
 #define DEFAULT_REFERENCE_FRAME "base"
 
@@ -45,7 +46,6 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
                              iCartesianSolver(NULL),
                              currentState(VOCAB_CC_NOT_CONTROLLING),
                              gain(DEFAULT_GAIN),
-                             maxJointVelocity(DEFAULT_QDOT_LIMIT),
                              waitPeriodMs(DEFAULT_WAIT_PERIOD_MS),
                              referenceFrame(ICartesianSolver::BASE_FRAME)
     {}
@@ -124,9 +124,10 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
     int currentState;
 
     double gain;
-    double maxJointVelocity;
     int waitPeriodMs;
 
+    std::vector<double> qdotMax;
+
     ICartesianSolver::reference_frame referenceFrame;
 };
 

libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp

@@ -21,9 +21,6 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config)
     gain = config.check("controllerGain", yarp::os::Value(DEFAULT_GAIN),
             "controller gain").asDouble();
 
-    maxJointVelocity = config.check("maxJointVelocity", yarp::os::Value(DEFAULT_QDOT_LIMIT),
-            "maximum joint velocity (meters/second or degrees/second)").asDouble();
-
     waitPeriodMs = config.check("waitPeriodMs", yarp::os::Value(DEFAULT_WAIT_PERIOD_MS),
             "wait command period (milliseconds)").asInt();
 
@@ -74,6 +71,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 +86,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));
     }

libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp

@@ -478,14 +478,6 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value)
         }
         gain = value;
         break;
-    case VOCAB_CC_CONFIG_MAX_JOINT_VEL:
-        if (value <= 0.0)
-        {
-            CD_ERROR("Maximum joint velocity cannot be negative nor zero.\n");
-            return false;
-        }
-        maxJointVelocity = value;
-        break;
     case VOCAB_CC_CONFIG_WAIT_PERIOD:
         if (value <= 0.0)
         {
@@ -519,9 +511,6 @@ bool roboticslab::AmorCartesianControl::getParameter(int vocab, double * value)
     case VOCAB_CC_CONFIG_GAIN:
         *value = gain;
         break;
-    case VOCAB_CC_CONFIG_MAX_JOINT_VEL:
-        *value = maxJointVelocity;
-        break;
     case VOCAB_CC_CONFIG_WAIT_PERIOD:
         *value = waitPeriodMs;
         break;
@@ -561,7 +550,6 @@ bool roboticslab::AmorCartesianControl::setParameters(const std::map<int, double
 bool roboticslab::AmorCartesianControl::getParameters(std::map<int, double> & params)
 {
     params.insert(std::make_pair(VOCAB_CC_CONFIG_GAIN, gain));
-    params.insert(std::make_pair(VOCAB_CC_CONFIG_MAX_JOINT_VEL, maxJointVelocity));
     params.insert(std::make_pair(VOCAB_CC_CONFIG_WAIT_PERIOD, waitPeriodMs));
     params.insert(std::make_pair(VOCAB_CC_CONFIG_FRAME, referenceFrame));
     return true;

libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp

@@ -8,6 +8,8 @@
 
 #include <ColorDebug.h>
 
+using namespace roboticslab;
+
 // -----------------------------------------------------------------------------
 
 namespace
@@ -24,7 +26,7 @@ namespace
 
 // -----------------------------------------------------------------------------
 
-int roboticslab::BasicCartesianControl::getCurrentState() const
+int BasicCartesianControl::getCurrentState() const
 {
     int tmp;
     currentStateReady.wait();
@@ -36,7 +38,7 @@ int roboticslab::BasicCartesianControl::getCurrentState() const
 
 // -----------------------------------------------------------------------------
 
-void roboticslab::BasicCartesianControl::setCurrentState(int value)
+void BasicCartesianControl::setCurrentState(int value)
 {
     currentStateReady.wait();
     currentState = value;
@@ -46,7 +48,7 @@ void roboticslab::BasicCartesianControl::setCurrentState(int value)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector<double> &q)
+bool BasicCartesianControl::checkJointLimits(const std::vector<double> &q)
 {
     for (unsigned int joint = 0; joint < numRobotJoints; joint++)
     {
@@ -56,7 +58,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;
         }
     }
@@ -66,19 +69,20 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector<doub
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector<double> &q, const std::vector<double> &qd)
+bool BasicCartesianControl::checkJointLimits(const std::vector<double> &q, const std::vector<double> &qdot)
 {
     for (unsigned int joint = 0; joint < numRobotJoints; joint++)
     {
         double value = q[joint];
 
-        if (value < qMin[joint] || value > qMax[joint])
+        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] (deg).\n",
+                    joint, value, qMin[joint], qMax[joint]);
             double midRange = (qMax[joint] + qMin[joint]) / 2;
 
             // Let the joint get away from its nearest limit.
-            if (qd.empty() || sgn(value - midRange) == sgn(qd[joint]))
+            if (sgn(value - midRange) == sgn(qdot[joint]))
             {
                 return false;
             }
@@ -90,13 +94,16 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector<doub
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::BasicCartesianControl::checkJointVelocities(const std::vector<double> &qdot)
+bool 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;
         }
     }
@@ -106,7 +113,7 @@ bool roboticslab::BasicCartesianControl::checkJointVelocities(const std::vector<
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::BasicCartesianControl::setControlModes(int mode)
+bool BasicCartesianControl::setControlModes(int mode)
 {
     std::vector<int> modes(numRobotJoints);
 
@@ -142,7 +149,7 @@ bool roboticslab::BasicCartesianControl::setControlModes(int mode)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::BasicCartesianControl::presetStreamingCommand(int command)
+bool BasicCartesianControl::presetStreamingCommand(int command)
 {
     setCurrentState(VOCAB_CC_NOT_CONTROLLING);
 
@@ -168,3 +175,57 @@ bool roboticslab::BasicCartesianControl::presetStreamingCommand(int command)
 }
 
 // -----------------------------------------------------------------------------
+
+void BasicCartesianControl::computeIsocronousSpeeds(const std::vector<double> & q, const std::vector<double> & qd,
+        std::vector<double> & qdot)
+{
+    double maxTime = 0.0;
+
+    //-- Find out the maximum time to move
+
+    for (int joint = 0; joint < numSolverJoints; joint++)
+    {
+        double distance = qd[joint] - q[joint];
+
+        CD_INFO("Distance (joint %d): %f\n", joint, std::abs(distance));
+
+        double targetTime;
+
+        if (distance >= 0.0 && qdotMax[joint] != 0.0)
+        {
+            targetTime = std::abs(distance / qdotMax[joint]);
+        }
+        else if (distance < 0.0 && qdotMin[joint] != 0.0)
+        {
+            targetTime = std::abs(distance / qdotMin[joint]);
+        }
+        else
+        {
+            CD_WARNING("Zero velocities sent, not moving.\n");
+            return;
+        }
+
+        if (targetTime > maxTime)
+        {
+            maxTime = targetTime;
+            CD_INFO("Candidate: %f\n", maxTime);
+        }
+    }
+
+    //-- Compute, store old and set joint velocities given this time
+
+    for (int joint = 0; joint < numRobotJoints; joint++)
+    {
+        if (joint >= numSolverJoints)
+        {
+            CD_INFO("qdot[%d] = 0.0 (forced)\n", joint);
+        }
+        else
+        {
+            qdot[joint] = std::abs(qd[joint] - q[joint]) / maxTime;
+            CD_INFO("qdot[%d] = %f\n", joint, qdot[joint]);
+        }
+    }
+}
+
+// -----------------------------------------------------------------------------

libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp

@@ -21,7 +21,6 @@
 #define DEFAULT_ROBOT "remote_controlboard"
 #define DEFAULT_INIT_STATE VOCAB_CC_NOT_CONTROLLING
 #define DEFAULT_GAIN 0.05
-#define DEFAULT_QDOT_LIMIT 10.0
 #define DEFAULT_DURATION 10.0
 #define DEFAULT_CMC_PERIOD_MS 50
 #define DEFAULT_WAIT_PERIOD_MS 30
@@ -128,7 +127,6 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
                               iPreciselyTimed(NULL),
                               referenceFrame(ICartesianSolver::BASE_FRAME),
                               gain(DEFAULT_GAIN),
-                              maxJointVelocity(DEFAULT_QDOT_LIMIT),
                               duration(DEFAULT_DURATION),
                               cmcPeriodMs(DEFAULT_CMC_PERIOD_MS),
                               waitPeriodMs(DEFAULT_WAIT_PERIOD_MS),
@@ -213,11 +211,12 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
     void setCurrentState(int value);
 
     bool checkJointLimits(const std::vector<double> &q);
-    bool checkJointLimits(const std::vector<double> &q, const std::vector<double> &qd);
+    bool checkJointLimits(const std::vector<double> &q, const std::vector<double> &qdot);
     bool checkJointVelocities(const std::vector<double> &qdot);
 
     bool setControlModes(int mode);
     bool presetStreamingCommand(int command);
+    void computeIsocronousSpeeds(const std::vector<double> & q, const std::vector<double> & qd, std::vector<double> & qdot);
 
     void handleMovj(const std::vector<double> &q);
     void handleMovl(const std::vector<double> &q);
@@ -241,7 +240,6 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
     ICartesianSolver::reference_frame referenceFrame;
 
     double gain;
-    double maxJointVelocity;
     double duration; // [s]
 
     int cmcPeriodMs;
@@ -271,6 +269,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
     bool cmcSuccess;
 
     std::vector<double> qMin, qMax;
+    std::vector<double> qdotMin, qdotMax;
 };
 
 }  // namespace roboticslab

libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp

@@ -13,9 +13,6 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config)
     gain = config.check("controllerGain", yarp::os::Value(DEFAULT_GAIN),
             "controller gain").asDouble();
 
-    maxJointVelocity = config.check("maxJointVelocity", yarp::os::Value(DEFAULT_QDOT_LIMIT),
-            "maximum joint velocity (meters/second or degrees/second)").asDouble();
-
     duration = config.check("trajectoryDuration", yarp::os::Value(DEFAULT_DURATION),
             "trajectory duration (seconds)").asDouble();
 
@@ -112,20 +109,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;