From a3600f79b637b9b63de6fda66fd0dd0a32a889b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Thu, 16 May 2019 17:35:48 +0200 Subject: [PATCH 1/3] Retrieve, store and use joint speed limits --- .../AmorCartesianControl.cpp | 4 +-- .../AmorCartesianControl.hpp | 4 +++ .../AmorCartesianControl/DeviceDriverImpl.cpp | 4 +++ .../BasicCartesianControl.cpp | 15 ++++++--- .../BasicCartesianControl.hpp | 1 + .../DeviceDriverImpl.cpp | 33 +++++++++++++++---- 6 files changed, 47 insertions(+), 14 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp index b687ad7d4..3304b1301 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp @@ -12,9 +12,9 @@ bool roboticslab::AmorCartesianControl::checkJointVelocities(const std::vector maxJointVelocity) + if (std::abs(qdot[i]) > qdotMax[i]) { - CD_ERROR("Maximum angular velocity hit: qdot[%d] = %f > %f [deg/s].\n", i, qdot[i], maxJointVelocity); + CD_ERROR("Maximum angular velocity hit: qdot[%d] = %f > %f [deg/s].\n", i, qdot[i], qdotMax[i]); return false; } } diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index 6e4454f19..71dac3607 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp @@ -3,6 +3,8 @@ #ifndef __AMOR_CARTESIAN_CONTROL_HPP__ #define __AMOR_CARTESIAN_CONTROL_HPP__ +#include + #include #include #include @@ -127,6 +129,8 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo double maxJointVelocity; int waitPeriodMs; + std::vector qdotMax; + ICartesianSolver::reference_frame referenceFrame; }; diff --git a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp index 621a4109a..17f1d04fe 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp @@ -74,6 +74,8 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config) CD_SUCCESS("Acquired AMOR handle!\n"); + qdotMax.resize(AMOR_NUM_JOINTS); + yarp::os::Bottle qMin, qMax; for (int i = 0; i < AMOR_NUM_JOINTS; i++) @@ -87,6 +89,8 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config) return false; } + qdotMax[i] = KinRepresentation::radToDeg(jointInfo.maxVelocity); + qMin.addDouble(KinRepresentation::radToDeg(jointInfo.lowerJointLimit)); qMax.addDouble(KinRepresentation::radToDeg(jointInfo.upperJointLimit)); } diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp index ad8b53177..d051ccae0 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp @@ -56,7 +56,8 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector qMax[joint] - epsilon) { - CD_WARNING("Joint near or out of limits: q[%d] = %f not in [%f,%f].\n", joint, value, qMin[joint], qMax[joint]); + CD_WARNING("Joint near or out of limits: q[%d] = %f not in [%f,%f] (def).\n", + joint, value, qMin[joint], qMax[joint]); return false; } } @@ -74,7 +75,8 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector qMax[joint]) { - CD_WARNING("Joint near or out of limits: q[%d] = %f not in [%f,%f].\n", joint, value, qMin[joint], qMax[joint]); + CD_WARNING("Joint near or out of limits: q[%d] = %f not in [%f,%f] (deg).\n", + joint, value, qMin[joint], qMax[joint]); double midRange = (qMax[joint] + qMin[joint]) / 2; // Let the joint get away from its nearest limit. @@ -92,11 +94,14 @@ bool roboticslab::BasicCartesianControl::checkJointLimits(const std::vector &qdot) { - for (unsigned int i = 0; i < qdot.size(); i++) + for (unsigned int joint = 0; joint < qdot.size(); joint++) { - if (std::abs(qdot[i]) > maxJointVelocity) + double value = qdot[joint]; + + if (value < qdotMin[joint] || value > qdotMax[joint]) { - CD_WARNING("Maximum angular velocity hit: qdot[%d] = |%f| > %f [deg/s].\n", i, qdot[i], maxJointVelocity); + CD_WARNING("Maximum angular velocity hit: qdot[%d] = %f not in [%f,%f] (deg/s).\n", + joint, value, qdotMin[joint], qdotMax[joint]); return false; } } diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index 261d40e07..90e2ebe8d 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -271,6 +271,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, bool cmcSuccess; std::vector qMin, qMax; + std::vector qdotMin, qdotMax; }; } // namespace roboticslab diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp index 003886460..ad8b12fff 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp @@ -112,20 +112,39 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config) qMin.resize(numRobotJoints); qMax.resize(numRobotJoints); + qdotMin.resize(numRobotJoints); + qdotMax.resize(numRobotJoints); + yarp::os::Bottle bMin, bMax; for (int joint = 0; joint < numRobotJoints; joint++) { - double min, max; - iControlLimits->getLimits(joint, &min, &max); + double _qMin, _qMax; + + if (!iControlLimits->getLimits(joint, &_qMin, &_qMax)) + { + CD_ERROR("Unable to retrieve position limits for joint %d.\n"); + return false; + } + + qMin[joint] = _qMin; + qMax[joint] = _qMax; + + double _qdotMin, _qdotMax; + + if (!iControlLimits->getVelLimits(joint, &_qdotMin, &_qdotMax)) + { + CD_ERROR("Unable to retrieve speed limits for joint %d.\n"); + return false; + } - qMin[joint] = min; - qMax[joint] = max; + qdotMin[joint] = _qdotMin; + qdotMax[joint] = _qdotMax; - bMin.addDouble(min); - bMax.addDouble(max); + CD_INFO("Joint %d limits: [%f,%f] [%f,%f]\n", joint, _qMin, _qMax, _qdotMin, _qdotMax); - CD_INFO("Joint %d limits: [%f,%f]\n", joint, min, max); + bMin.addDouble(_qMin); + bMax.addDouble(_qMax); } yarp::os::Property solverOptions; From 89020afa781ec6d57db4ab911414f2d4014207cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Thu, 16 May 2019 18:54:08 +0200 Subject: [PATCH 2/3] Drop max joint velocity config, fetch from robot * https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/159 * https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/173 --- .../AmorCartesianControl.hpp | 3 - .../AmorCartesianControl/DeviceDriverImpl.cpp | 3 - .../ICartesianControlImpl.cpp | 12 ---- .../BasicCartesianControl.cpp | 70 +++++++++++++++++-- .../BasicCartesianControl.hpp | 4 +- .../DeviceDriverImpl.cpp | 3 - .../ICartesianControlImpl.cpp | 47 +------------ .../CartesianControlServer/RpcResponder.cpp | 4 -- libraries/YarpPlugins/ICartesianControl.h | 1 - 9 files changed, 66 insertions(+), 81 deletions(-) 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] From 99ab1aa21336eb8d0a1729c8091df25200df1794 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Mon, 11 Mar 2019 14:18:47 +0100 Subject: [PATCH 3/3] Check velocity on movi, restore eps in joint check --- .../BasicCartesianControl/BasicCartesianControl.cpp | 6 +++--- .../BasicCartesianControl/BasicCartesianControl.hpp | 2 +- .../BasicCartesianControl/ICartesianControlImpl.cpp | 13 ++++++++----- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp index a84dc7dbb..1c9dc0204 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp @@ -69,20 +69,20 @@ bool BasicCartesianControl::checkJointLimits(const std::vector &q) // ----------------------------------------------------------------------------- -bool BasicCartesianControl::checkJointLimits(const std::vector &q, const std::vector &qd) +bool BasicCartesianControl::checkJointLimits(const std::vector &q, const std::vector &qdot) { for (unsigned int joint = 0; joint < numRobotJoints; joint++) { double value = q[joint]; - if (value < qMin[joint] || value > qMax[joint]) + if (value < qMin[joint] + epsilon || value > qMax[joint] - epsilon) { CD_WARNING("Joint near or out of limits: q[%d] = %f not in [%f,%f] (deg).\n", joint, value, qMin[joint], qMax[joint]); double midRange = (qMax[joint] + qMin[joint]) / 2; // Let the joint get away from its nearest limit. - if (qd.empty() || sgn(value - midRange) == sgn(qd[joint])) + if (sgn(value - midRange) == sgn(qdot[joint])) { return false; } diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index be66ec70b..a5e6de5df 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -211,7 +211,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, void setCurrentState(int value); bool checkJointLimits(const std::vector &q); - bool checkJointLimits(const std::vector &q, const std::vector &qd); + bool checkJointLimits(const std::vector &q, const std::vector &qdot); bool checkJointVelocities(const std::vector &qdot); bool setControlModes(int mode); diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index e1f777a12..6a65bbd2c 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -576,24 +576,27 @@ void roboticslab::BasicCartesianControl::movi(const std::vector &x) return; } - std::vector qd(numRobotJoints); + std::vector qdot(numRobotJoints); + + double interval = yarp::os::Time::now() - movementStartTime; for (int i = 0; i < numRobotJoints; i++) { - qd[i] = q[i] - currentQ[i]; + qdot[i] = (q[i] - currentQ[i]) / interval; } - if (!checkJointLimits(currentQ, qd)) + if (!checkJointLimits(currentQ, qdot) || !checkJointVelocities(qdot)) { - CD_ERROR("checkJointLimits failed, stopping.\n"); + CD_ERROR("Joint position or velocity limits exceeded, not moving.\n"); return; } if (!iPositionDirect->setPositions(q.data())) { CD_ERROR("setPositions failed.\n"); - return; } + + movementStartTime = yarp::os::Time::now(); } // -----------------------------------------------------------------------------