diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index 71dac3607..5f937e8d2 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp +++ b/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 qdotMax; diff --git a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp index 17f1d04fe..3af85c7c6 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp +++ b/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(); diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index 0f5dd2a0f..feffdab17 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/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 & 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; diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp index d051ccae0..a84dc7dbb 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp @@ -8,6 +8,8 @@ #include +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 &q) +bool BasicCartesianControl::checkJointLimits(const std::vector &q) { for (unsigned int joint = 0; joint < numRobotJoints; joint++) { @@ -67,7 +69,7 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector &q, const std::vector &qd) +bool BasicCartesianControl::checkJointLimits(const std::vector &q, const std::vector &qd) { for (unsigned int joint = 0; joint < numRobotJoints; joint++) { @@ -92,7 +94,7 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector &qdot) +bool BasicCartesianControl::checkJointVelocities(const std::vector &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 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 & q, const std::vector & qd, + std::vector & 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]); + } + } +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index 90e2ebe8d..be66ec70b 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/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 & q, const std::vector & qd, std::vector & qdot); void handleMovj(const std::vector &q); void handleMovl(const std::vector &q); @@ -241,7 +240,6 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, ICartesianSolver::reference_frame referenceFrame; double gain; - double maxJointVelocity; double duration; // [s] int cmcPeriodMs; diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp index ad8b12fff..67752257a 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp +++ b/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(); diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index 5d54b97e5..e1f777a12 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -94,40 +94,9 @@ bool roboticslab::BasicCartesianControl::movj(const std::vector &xd) return false; } - //-- Find out the maximum time to move - double max_time = 0; + std::vector 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 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 & 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)); diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp index ca1a9ec07..5e98ed9b8 100644 --- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp +++ b/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(""); diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index 4a9723df1..1967b5de5 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/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]