Add new streaming command period config parameter

roboticslab-uc3m/yarp-devices#198 (comment)
roboticslab-uc3m · May 21, 2019 · 67106db · 67106db
1 parent 4436280
commit 67106db
Show file tree

Hide file tree

Showing 5 changed files with 34 additions and 11 deletions.
diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp
@@ -24,6 +24,7 @@
 #define DEFAULT_DURATION 10.0
 #define DEFAULT_CMC_PERIOD_MS 50
 #define DEFAULT_WAIT_PERIOD_MS 30
+#define DEFAULT_STREAMING_PERIOD_MS 50
 #define DEFAULT_REFERENCE_FRAME "base"
 #define DEFAULT_STREAMING_PRESET 0
 
@@ -130,6 +131,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
                               duration(DEFAULT_DURATION),
                               cmcPeriodMs(DEFAULT_CMC_PERIOD_MS),
                               waitPeriodMs(DEFAULT_WAIT_PERIOD_MS),
+                              streamingPeriodMs(DEFAULT_STREAMING_PERIOD_MS),
                               numRobotJoints(0),
                               numSolverJoints(0),
                               currentState(DEFAULT_INIT_STATE),
@@ -244,6 +246,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
 
     int cmcPeriodMs;
     int waitPeriodMs;
+    int streamingPeriodMs;
     int numRobotJoints, numSolverJoints;
 
     /** State encoded as a VOCAB which can be stored as an int */

diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
@@ -22,6 +22,9 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config)
     waitPeriodMs = config.check("waitPeriodMs", yarp::os::Value(DEFAULT_WAIT_PERIOD_MS),
             "wait command period (milliseconds)").asInt32();
 
+    streamingPeriodMs = config.check("streamingPeriodMs", yarp::os::Value(DEFAULT_STREAMING_PERIOD_MS),
+            "streaming command send period (milliseconds)").asInt32();
+
     std::string referenceFrameStr = config.check("referenceFrame", yarp::os::Value(DEFAULT_REFERENCE_FRAME),
              "reference frame (base|tcp)").asString();
 

diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp
@@ -576,16 +576,16 @@ void roboticslab::BasicCartesianControl::movi(const std::vector<double> &x)
         return;
     }
 
-    std::vector<double> qdiff(numRobotJoints);
+    std::vector<double> qdot(numRobotJoints);
 
     for (int i = 0; i < numRobotJoints; i++)
     {
-        qdiff[i] = q[i] - currentQ[i];
+        qdot[i] = (q[i] - currentQ[i]) / streamingPeriodMs;
     }
 
-    if (!checkJointLimits(currentQ, qdiff))
+    if (!checkJointLimits(currentQ, qdot) || !checkJointVelocities(qdot))
     {
-        CD_ERROR("Joint position exceeded, not moving.\n");
+        CD_ERROR("Joint position or velocity limits exceeded, not moving.\n");
         return;
     }
 
@@ -655,6 +655,14 @@ bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value)
         }
         streamingCommand = value;
         break;
+    case VOCAB_CC_CONFIG_STREAMING_PERIOD:
+        if (value <= 0.0)
+        {
+            CD_ERROR("Streaming command rate cannot be negative nor zero.\n");
+            return false;
+        }
+        streamingPeriodMs = value;
+        break;
     default:
         CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str());
         return false;
@@ -687,6 +695,9 @@ bool roboticslab::BasicCartesianControl::getParameter(int vocab, double * value)
     case VOCAB_CC_CONFIG_STREAMING_CMD:
         *value = streamingCommand;
         break;
+    case VOCAB_CC_CONFIG_STREAMING_PERIOD:
+        *value = streamingPeriodMs;
+        break;
     default:
         CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str());
         return false;
@@ -725,6 +736,7 @@ bool roboticslab::BasicCartesianControl::getParameters(std::map<int, double> & p
     params.insert(std::make_pair(VOCAB_CC_CONFIG_WAIT_PERIOD, waitPeriodMs));
     params.insert(std::make_pair(VOCAB_CC_CONFIG_FRAME, referenceFrame));
     params.insert(std::make_pair(VOCAB_CC_CONFIG_STREAMING_CMD, streamingCommand));
+    params.insert(std::make_pair(VOCAB_CC_CONFIG_STREAMING_PERIOD, streamingPeriodMs));
     return true;
 }
 

diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp
@@ -183,6 +183,10 @@ void roboticslab::RpcResponder::makeUsage()
     addUsage(ss.str().c_str(), ss_cmd.str().c_str());
 
     ss.str("");
+
+    ss << "... [" << yarp::os::Vocab::decode(VOCAB_CC_CONFIG_STREAMING_PERIOD) << "] value";
+    addUsage(ss.str().c_str(), "(config param) streaming command send period [ms]");
+    ss.str("");
 }
 
 // -----------------------------------------------------------------------------

diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h
@@ -107,13 +107,14 @@
  */
 
 // 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_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]
-#define VOCAB_CC_CONFIG_FRAME ROBOTICSLAB_VOCAB('c','p','f',0)            ///< Reference frame
-#define VOCAB_CC_CONFIG_STREAMING_CMD ROBOTICSLAB_VOCAB('c','p','s','c')  ///< Preset streaming command
+#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_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]
+#define VOCAB_CC_CONFIG_FRAME ROBOTICSLAB_VOCAB('c','p','f',0)              ///< Reference frame
+#define VOCAB_CC_CONFIG_STREAMING_CMD ROBOTICSLAB_VOCAB('c','p','s','c')    ///< Preset streaming command
+#define VOCAB_CC_CONFIG_STREAMING_PERIOD ROBOTICSLAB_VOCAB('c','p','s','p') ///< Streaming command send period [ms]
 
 /** @} */