diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index a19b54964..709f4a164 100644 --- 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 index 5b0abb783..11209c4a9 100644 --- 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 index 540b4af2d..3a9847697 100644 --- 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 & pa { params.insert(std::pair(VOCAB_CC_CONFIG_GAIN, gain)); params.insert(std::pair(VOCAB_CC_CONFIG_MAX_JOINT_VEL, maxJointVelocity)); + params.insert(std::pair(VOCAB_CC_CONFIG_WAIT_PERIOD, waitPeriodMs)); params.insert(std::pair(VOCAB_CC_CONFIG_FRAME, referenceFrame)); return true; } diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index dbd37c78a..343e61c71 100644 --- 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 index 6f23f0b7d..84fe39b92 100644 --- 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 index e1c3b420f..d04beb7e9 100644 --- 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 & p params.insert(std::pair(VOCAB_CC_CONFIG_MAX_JOINT_VEL, maxJointVelocity)); params.insert(std::pair(VOCAB_CC_CONFIG_TRAJ_DURATION, duration)); params.insert(std::pair(VOCAB_CC_CONFIG_CMC_RATE, cmcRateMs)); + params.insert(std::pair(VOCAB_CC_CONFIG_WAIT_PERIOD, waitPeriodMs)); params.insert(std::pair(VOCAB_CC_CONFIG_FRAME, referenceFrame)); return true; } diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp index 618643e86..81609102a 100644 --- 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 index 9c0fdd479..4d5c38350 100644 --- 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 /** @} */