Add new wait period CC config option plus implementation

roboticslab-uc3m · Feb 3, 2018 · 0d7375a · 0d7375a
1 parent 566278b
commit 0d7375a
Show file tree

Hide file tree

Showing 8 changed files with 37 additions and 2 deletions.
diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp
@@ -17,6 +17,7 @@
 
 #define DEFAULT_GAIN 0.05
 #define DEFAULT_QDOT_LIMIT 10.0
+#define DEFAULT_WAIT_PERIOD_MS 30
 #define DEFAULT_REFERENCE_FRAME "base"
 
 namespace roboticslab
@@ -45,6 +46,7 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
                              currentState(VOCAB_CC_NOT_CONTROLLING),
                              gain(DEFAULT_GAIN),
                              maxJointVelocity(DEFAULT_QDOT_LIMIT),
+                             waitPeriodMs(DEFAULT_WAIT_PERIOD_MS),
                              referenceFrame(BASE_FRAME)
     {}
 
@@ -125,6 +127,7 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
 
     double gain;
     double maxJointVelocity;
+    int waitPeriodMs;
 
     reference_frame referenceFrame;
 };

diff --git a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp
@@ -21,6 +21,9 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config)
     maxJointVelocity = config.check("maxJointVelocity", yarp::os::Value(DEFAULT_QDOT_LIMIT),
             "maximum joint velocity").asDouble();
 
+    waitPeriodMs = config.check("waitPeriodMs", yarp::os::Value(DEFAULT_WAIT_PERIOD_MS),
+            "wait period [ms]").asInt();
+
     std::string referenceFrameStr = config.check("referenceFrame", yarp::os::Value(DEFAULT_REFERENCE_FRAME),
             "reference frame").asString();
 

diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp
@@ -277,7 +277,7 @@ bool roboticslab::AmorCartesianControl::wait(double timeout)
             break;
         }
 
-        yarp::os::Time::delay(0.5);  // seconds
+        yarp::os::Time::delay(waitPeriodMs / 1000.0);
     }
     while (status != AMOR_MOVEMENT_STATUS_FINISHED);
 
@@ -424,6 +424,14 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value)
         }
         maxJointVelocity = value;
         break;
+    case VOCAB_CC_CONFIG_WAIT_PERIOD:
+        if (value <= 0.0)
+        {
+            CD_ERROR("Wait period cannot be negative nor zero.\n");
+            return false;
+        }
+        waitPeriodMs = value;
+        break;
     case VOCAB_CC_CONFIG_FRAME:
         if (value != BASE_FRAME && value != TCP_FRAME)
         {
@@ -452,6 +460,9 @@ bool roboticslab::AmorCartesianControl::getParameter(int vocab, double * value)
     case VOCAB_CC_CONFIG_MAX_JOINT_VEL:
         *value = maxJointVelocity;
         break;
+    case VOCAB_CC_CONFIG_WAIT_PERIOD:
+        *value = waitPeriodMs;
+        break;
     case VOCAB_CC_CONFIG_FRAME:
         *value = referenceFrame;
         break;
@@ -483,6 +494,7 @@ bool roboticslab::AmorCartesianControl::getParameters(std::map<int, double> & pa
 {
     params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_GAIN, gain));
     params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_MAX_JOINT_VEL, maxJointVelocity));
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_WAIT_PERIOD, waitPeriodMs));
     params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_FRAME, referenceFrame));
     return true;
 }

diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp
@@ -23,6 +23,7 @@
 #define DEFAULT_QDOT_LIMIT 10.0
 #define DEFAULT_DURATION 10.0
 #define DEFAULT_CMC_RATE_MS 50
+#define DEFAULT_WAIT_PERIOD_MS 30
 #define DEFAULT_REFERENCE_FRAME "base"
 
 namespace roboticslab
@@ -208,6 +209,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC
         double maxJointVelocity;
         double duration; // [s]
         int cmcRateMs;
+        int waitPeriodMs;
         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
@@ -14,6 +14,7 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config) {
     maxJointVelocity = config.check("maxJointVelocity",yarp::os::Value(DEFAULT_QDOT_LIMIT),"maximum joint velocity").asDouble();
     duration = config.check("trajectoryDuration",yarp::os::Value(DEFAULT_DURATION),"trajectory duration").asDouble();
     cmcRateMs = config.check("cmcRateMs",yarp::os::Value(DEFAULT_CMC_RATE_MS),"CMC rate [ms]").asInt();
+    waitPeriodMs = config.check("waitPeriodMs",yarp::os::Value(DEFAULT_WAIT_PERIOD_MS),"wait period [ms]").asInt();
 
     std::string referenceFrameStr = config.check("referenceFrame", yarp::os::Value(DEFAULT_REFERENCE_FRAME),
                 "reference frame").asString();

diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp
@@ -320,7 +320,7 @@ bool roboticslab::BasicCartesianControl::wait(double timeout)
             break;
         }
 
-        yarp::os::Time::delay(0.5);
+        yarp::os::Time::delay(waitPeriodMs / 1000.0);
         state = getCurrentState();
     }
 
@@ -476,6 +476,14 @@ bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value)
         }
         cmcRateMs = value;
         break;
+    case VOCAB_CC_CONFIG_WAIT_PERIOD:
+        if (value <= 0.0)
+        {
+            CD_ERROR("Wait period cannot be negative nor zero.\n");
+            return false;
+        }
+        waitPeriodMs = value;
+        break;
     case VOCAB_CC_CONFIG_FRAME:
         if (value != BASE_FRAME && value != TCP_FRAME)
         {
@@ -510,6 +518,9 @@ bool roboticslab::BasicCartesianControl::getParameter(int vocab, double * value)
     case VOCAB_CC_CONFIG_CMC_RATE:
         *value = cmcRateMs;
         break;
+    case VOCAB_CC_CONFIG_WAIT_PERIOD:
+        *value = waitPeriodMs;
+        break;
     case VOCAB_CC_CONFIG_FRAME:
         *value = referenceFrame;
         break;
@@ -543,6 +554,7 @@ bool roboticslab::BasicCartesianControl::getParameters(std::map<int, double> & p
     params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_MAX_JOINT_VEL, maxJointVelocity));
     params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_TRAJ_DURATION, duration));
     params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_CMC_RATE, cmcRateMs));
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_WAIT_PERIOD, waitPeriodMs));
     params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_FRAME, referenceFrame));
     return true;
 }

diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp
@@ -101,6 +101,7 @@ void roboticslab::RpcResponder::makeUsage()
     addUsage("... [cpjv] value", "(config param) maximum joint velocity");
     addUsage("... [cptd] value", "(config param) trajectory duration");
     addUsage("... [cpcr] value", "(config param) CMC rate [ms]");
+    addUsage("... [cpwp] value", "(config param) check period of 'wait' command [ms]");
     addUsage("... [cpf] [cpfb]", "(config param) reference frame (base)");
     addUsage("... [cpf] [cpft]", "(config param) reference frame (TCP)");
 }

diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h
@@ -105,6 +105,7 @@
 #define VOCAB_CC_CONFIG_MAX_JOINT_VEL VOCAB4('c','p','j','v') ///< Maximum joint velocity
 #define VOCAB_CC_CONFIG_TRAJ_DURATION VOCAB4('c','p','t','d') ///< Trajectory duration
 #define VOCAB_CC_CONFIG_CMC_RATE VOCAB4('c','p','c','r')      ///< CMC rate [ms]
+#define VOCAB_CC_CONFIG_WAIT_PERIOD VOCAB4('c','p','w','p')   ///< Check period of 'wait' command [ms]
 #define VOCAB_CC_CONFIG_FRAME VOCAB3('c','p','f')             ///< Reference frame
 
 /** @} */