Drop max joint velocity config, fetch from robot

* #159
* #173

libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp

@@ -18,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"
 
@@ -47,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)
     {}
@@ -126,7 +124,6 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
     int currentState;
 
     double gain;
-    double maxJointVelocity;
     int waitPeriodMs;
 
     std::vector<double> qdotMax;

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();
 

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++)
     {
@@ -67,7 +69,7 @@ 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> &qd)
 {
     for (unsigned int joint = 0; joint < numRobotJoints; joint++)
     {
@@ -92,7 +94,7 @@ 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 joint = 0; joint < qdot.size(); joint++)
     {
@@ -111,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);
 
@@ -147,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);
 
@@ -173,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),
@@ -218,6 +216,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
 
     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;

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();
 

libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp

@@ -94,40 +94,9 @@ bool roboticslab::BasicCartesianControl::movj(const std::vector<double> &xd)
         return false;
     }
 
-    //-- Find out the maximum time to move
-    double max_time = 0;
+    std::vector<double> vmo(numRobotJoints);
 
-    for (int joint = 0; joint < numSolverJoints; joint++)
-    {
-        CD_INFO("dist[%d]: %f\n", joint, std::abs(qd[joint] - currentQ[joint]));
-
-        if (std::abs((qd[joint] - currentQ[joint]) / maxJointVelocity) > max_time)
-        {
-            max_time = std::abs((qd[joint] - currentQ[joint]) / maxJointVelocity);
-            CD_INFO(" -->candidate: %f\n", max_time);
-        }
-    }
-
-    CD_INFO("max_time[final]: %f\n", max_time);
-
-    //-- Compute, store old and set joint velocities given this time
-    std::vector<double> vmo;
-
-    for (int joint = 0; joint < numRobotJoints; joint++)
-    {
-        if (joint >= numSolverJoints)
-        {
-            vmo.push_back(0.0);
-            CD_INFO("vmo[%d]: 0.0 (forced)\n", joint);
-        }
-        else
-        {
-            vmo.push_back(std::abs(qd[joint] - currentQ[joint]) / max_time);
-            CD_INFO("vmo[%d]: %f\n", joint, vmo[joint]);
-        }
-    }
-
-    vmoStored.resize(numRobotJoints);
+    computeIsocronousSpeeds(currentQ, qd, vmo);
 
     if (!iPositionControl->getRefSpeeds(vmoStored.data()))
     {
@@ -647,14 +616,6 @@ bool roboticslab::BasicCartesianControl::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_TRAJ_DURATION:
         if (value <= 0.0)
         {
@@ -712,9 +673,6 @@ bool roboticslab::BasicCartesianControl::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_TRAJ_DURATION:
         *value = duration;
         break;
@@ -763,7 +721,6 @@ bool roboticslab::BasicCartesianControl::setParameters(const std::map<int, doubl
 bool roboticslab::BasicCartesianControl::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_TRAJ_DURATION, duration));
     params.insert(std::make_pair(VOCAB_CC_CONFIG_CMC_PERIOD, cmcPeriodMs));
     params.insert(std::make_pair(VOCAB_CC_CONFIG_WAIT_PERIOD, waitPeriodMs));

libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp

@@ -153,10 +153,6 @@ void roboticslab::RpcResponder::makeUsage()
     addUsage(ss.str().c_str(), "(config param) controller gain");
     ss.str("");
 
-    ss << "... [" << yarp::os::Vocab::decode(VOCAB_CC_CONFIG_MAX_JOINT_VEL) << "] value";
-    addUsage(ss.str().c_str(), "(config param) maximum joint velocity");
-    ss.str("");
-
     ss << "... [" << yarp::os::Vocab::decode(VOCAB_CC_CONFIG_TRAJ_DURATION) << "] value";
     addUsage(ss.str().c_str(), "(config param) trajectory duration");
     ss.str("");

libraries/YarpPlugins/ICartesianControl.h

@@ -109,7 +109,6 @@
 // Controller configuration (parameter keys)
 #define VOCAB_CC_CONFIG_PARAMS ROBOTICSLAB_VOCAB('p','r','m','s')        ///< Parameter group
 #define VOCAB_CC_CONFIG_GAIN ROBOTICSLAB_VOCAB('c','p','c','g')          ///< Controller gain
-#define VOCAB_CC_CONFIG_MAX_JOINT_VEL ROBOTICSLAB_VOCAB('c','p','j','v') ///< Maximum joint velocity
 #define VOCAB_CC_CONFIG_TRAJ_DURATION ROBOTICSLAB_VOCAB('c','p','t','d') ///< Trajectory duration
 #define VOCAB_CC_CONFIG_CMC_PERIOD ROBOTICSLAB_VOCAB('c','p','c','p')    ///< CMC period [ms]
 #define VOCAB_CC_CONFIG_WAIT_PERIOD ROBOTICSLAB_VOCAB('c','p','w','p')   ///< Check period of 'wait' command [ms]